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ABSTRACT 


This  thesis  supports  ongoing  ONR  research  in  the  area  of  Autonomous 
Underwater  Vehicles  (AUVs)  and  Mine  Warfare.  It  shows  a  simulation  of  a  two-vehicle 
autonomous  rendezvous  using  both  along  track  and  cross  track  position  controllers. 
Conducting  open  water  experiments  with  the  ARIES  AUV  identified  the  added  mass 
matrix  and  hydrodynamic  coefficients  of  the  longitudinal  equation  of  motion.  The  results 
indicate  that  it  will  be  possible  to  maneuver  an  AUV  to  a  specific  rendezvous  point  at  a 
specified  time.  Two-vehicle  rendezvous  maneuvers  are  likely  to  be  needed  in  multi¬ 
vehicle  operations  when  data  transfer  between  range-limited  communications  modems 
are  used. 
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I.  INTRODUCTION 


A.  BACKGROUND 

Research  in  the  field  of  autonomous  underwater  vehicles  (AUVs)  at  the  Naval 
Postgraduate  School  (NPS)  has  progressed  steadily  since  the  inception  of  the  Center  for 
AUV  Research  in  1987.  The  operational  capabilities  and  sophistication  of  software  and 
hardware  has  greatly  increased  with  each  new  generation  of  vehicle.  From  humble 
beginnings  in  swimming  pools  to  open  ocean  operation,  these  vehicles  have  been  at  the 
forefront  of  AUV  research. 

The  unique  ability  of  AUVs  to  operate  in  very  shallow  water  (VSW),  where 
depths  range  from  ten  to  thirty  feet,  enables  the  Mine  Warfare  (MIW)  Commander  to 
search,  detect,  and  classify  mines  effortlessly  and  safely  when  compared  to  current 
practice  Navy  Special  Operations  or  Special  Warfare  Teams.  In  an  effort  to  expand  the 
search  area  and  provide  the  MIW  Commander  with  near  real-time  information,  the  use  of 
multiple  vehicles  having  the  ability  to  communicate  between  each  other  is  the  logical 
solution.  The  Acoustic  Radio  Interactive  Exploratory  Server  (ARIES)  AUV  is  designed 
to  operate  as  a  communications  server  vehicle.  It  is  outfitted  with  the  Elorida  Atlantic 
University  (EAU)  acoustic  modem  and  has  the  capability  to  act  as  a  command  and 
control  vehicle  for  numerous  vehicles  while  operating  in  the  search  area.  Equipped  with 
a  radio  modem,  when  surfaced,  ARIES  has  the  ability  to  send  and  receive  data  to  the 
MIW  Commander  embarked  offshore. 

Current  modem  technology  has  limitations,  requiring  increases  in  both  data  rate 
and  range.  This  will  also  require  an  increase  in  power  and  size  of  the  modem.  By  using 
a  mobile  server,  such  as  ARIES,  close  proximity,  high-speed  data  transfers  will  achieve 
the  same  results  as  a  long-range  modem.  Current  employment  of  the  ARIES  is  shown  in 
Eigure  I.  Command  and  control  of  ARIES  is  accomplished  using  a  Boston  Whaler  as  a 
relay  station.  Radio  ranges  of  four  to  six  nautical  miles  with  data  transfer  rates  of  30,000 
bits/sec  are  routine. 
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Figure  1.  Current  Employment  of  ARIES  (Erom;  Healey,  2001) 


Possible  employment  of  ARIES  is  shown  in  Eigure  2.  Here  ARIES  will 
rendezvous  with  other  vehicles,  such  as  the  Remote  Environmental  Measuring  Units 
(REMUS),  conduct  file  transfer  operations  and  then  relay  the  information  to  the 
command  and  control  base. 
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Figure  2.  Possible  Employment  of  ARIES  (Erom;  Flealey,  2001) 


In  order  to  accomplish  the  rendezvous,  ARIES  must  have  the  capability  to  arrive 
at  a  certain  location  at  a  certain  time.  This  is  known  as  trajectory  planning. 

B.  SCOPE  OF  THIS  WORK 

Trajectory  planning  has  not  been  researched  in  the  case  of  AUVs  and  has  only 
been  touched  on  in  the  robotics  community.  Kant  and  Zucker  (1986)  illustrated  the 
everyday  version  of  the  trajectory-planning  problem  as  simply  being,  “How  does  one  get 
from  here  to  there”?  This  is  a  problem  that  humans  solve  so  often  and  intuitively  that  the 
underlying  complexity  is  given  little  or  no  thought. 

The  focus  of  this  thesis  is  two-fold; 

1.  To  develop  a  new  behavior  for  ARIES  -  that  of  arriving  at  a  dehned  place  in 
space  and  time;  and 

2.  To  extend  cross-track  error  guidance  algorithms  (Marco,  2000)  to  close  along 
track  as  well  as  cross  track  errors  resulting  in  a  trajectory  controller. 
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Chapter  II  will  focus  on  the  equations  of  motion  for  an  AUV  and  the  associated 
steering  control  laws.  Chapter  III  will  discuss  trajectory  planning  theory  and 
implementation.  Chapter  IV  will  discuss  the  theory  and  design  of  a  sophisticated  speed 
controller  to  complement  the  trajectory  controller  discussed  in  the  previous  chapter. 
Chapter  V  will  present  simulation  results  from  the  implementation  of  the  complete 
trajectory/speed  controller  combination  and  Chapter  VI  will  offer  conclusions  and 
recommendations  for  future  study. 
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II.  EQUATIONS  OF  MOTION  AND  AUV  MODELING 

A,  GENERALIZED  STEERING  EQUATIONS  OF  MOTION 

This  section  describes  the  equations  of  motion  that  are  the  basis  for  the  model 
used  to  construct  ARIES  steering  controllers. 

Using  a  Newton-Euler  approach,  Healey,  (1995)  derives  the  equations  of  six 
degree  of  freedom  motion  as: 

SURGE  EQUATION  OF  MOTION 

m[u,-v,r  +  w^q-XG[q^  +r^]+yQ{pq-r)+ZQ{pr  +  q)]+{W-B)%mQ  =  Xf  (I) 

SWAY  EQUATION  OF  MOTION 

m[  +u^r-Wi.p  +  XQ{pq  +  r)-yQ{p^  +  r^  j+Zg  (gr-/?)]  -(tF-5)cos  9  sin  (|)  =  (2) 

HEAVE  EQUATION  OF  MOTION 

m[  -  u^q  +  v^p  +  XQ{pr-q)+yQ{qr  +  p)-Zq  [p^  +q^)^  +(tT-5)cos9cos(|)  =  Zf  (3) 

ROLL  EQUATION  OF  MOTION 

4/'  +  (U  - 1  yhr  + 1  ^y{pr  -  q)- 1  -  r^]- 1  ^^{pq  +  r)+m\y  q{w-u  ^q  +  v  ^p)  (4) 

-  zq  {vj.  +u^r-  {y^W  -  ^^^^(cos  9  cos  (|)  +  {zqW-  ZgB)co$  9  sin  (|)  = 

PITCH  EQUATION  OF  MOTION 

lyq  +  i^z -Iz)pr-Ixy{qr  +  p)+IyApq-r)+Ixz{p^ -r^]-m[xQ{w-u^q  +  v^p)  (5) 

-  zg  (m^  -  v^r  +  w^9)]+  (xgtP  -  x^b)co$  9  cos  (|)  +  (zqJV-  z^B)$in  Q  =  My 
YAW  EQUATION  OF  MOTION 

Iz^  +  {ly  -  I  x)p<l  -  I  xyip^  -‘l^)-Iyz{p>'  +  ^)+Ixz{‘l>'- p)+m[xGivr  +U,r-W^p)  (6) 

-yai^r  -v^r  +  w^q^-{xQW -XBB)c<i%Q%miSf-{yQW -yBB)%mQ  =  N f 

Where: 

Ur,  Vr,  Wr  =  componcnt  velocities  for  a  body  fixed  system  with  respect  to  the  water 
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p,q,  r  =  component  angular  velocities  for  a  body  fixed  system 

W  =  weight 

B  =  buoyancy 

I  =  mass  moment  of  inertia  terms 

xb,  ye,  Zb  =  position  difference  between  geometric  center  of  ARIES  and  center  of 
buoyancy 

xg,  yo,  zg  =  position  difference  between  geometric  center  of  ARIES  and  center  of 
gravity 

Xf,  Yf,  Zf,  Kp,  Mf,  Nf  =  sums  of  all  external  forces  acting  on  ARIES  in  the  particular 
body  fixed  direction 

Healey  (1995)  further  simplifies  Equations  1  thru  6  by  assuming  that  the  center  of 
mass  of  the  vehicle  lies  below  the  origin  (zg  is  positive)  while  xg  and  yG  are  zero,  and 
that  the  vehicle  is  symmetric  in  its  inertial  properties.  It  is  also  assumed  that  motions  in 
the  vertical  are  negligible  (i.e.  [wr,  p,  q,  r,  Z,  (|),  0]  =  0)  and  that  Ur  equals  the  forward 
speed,  Uq.  The  simplified  equations  of  motion  are  thus: 


Ur  =  Uo 

(V) 

mv^  = -rnllgr  +  AY f{t) 

(8) 

P,r  =  AN^.{t) 

(9) 

\j/=  r 

(10) 

X  =  Ug  cos\|/-v^  sin\|/  +  C/^^ 

(11) 

F  =  sin  \|/  -  cos  \|/  +  U^y 

(12) 

Johnson  (2001)  defines  AYj.(t)  and  ANj.(t)  as  forces  that  are  functions  of  the 

vehicles  dynamic  parameters.  Through  the  assumption  of  ‘small’  motions 

‘hydrodynamic  coefficients’  can  be  defined  related  to  the  individual  motion  components. 

The  expression  for  the  transverse  force  is: 

Yf  =TgV,  +E  V,  +Y/.r  +  Y^r 

(13) 

and  for  the  expression  for  the  rotational  force  is: 

N ^  +N^,  v,.  +  N +  N^r 

(15) 

This  leads  to: 
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and 


Y.:  = 


dY 


f  . 


dv^  ’ 


aVf 

3vj 


aVf . 


N,,  =- 


dN 


f  . 


3v, 


N„  =■ 


dN 


f  . 


3v, 


N,  =- 


aN 


f . 


dr 


N. 


aNf . 


Where: 

7.  =  added  mass  in  sway  coeffieient 
Y-  =  added  mass  in  yaw  coeffieient 

=  coefficient  of  sway  force  induced  by  side  slip 
Y^  =  coefficient  of  sway  force  induced  by  yaw 

=  added  mass  moment  of  inertia  in  sway  coefficient 
=  added  mass  moment  of  inertia  in  yaw  coefficient 
=  coefficient  of  sway  moment  from  side  slip 
=  coefficient  of  sway  moment  from  yaw 

In  addition,  the  action  of  the  rudder  will  produce  forces  that  when  linearized  are:  Yg6^  (t) 
and  Ng5j(t) .  The  dynamics  of  the  vehicle  are  thus  defined  as: 

mv^  =  -mU gF  +  Y^  +Y^  +Y j.r  +  Yj.r  +  Y^h ^(f)  (16) 

Izzr  =  NiV^  +N^v^  +  N  i.r  +  N  ,r  +  N  ,(t)  (17) 

^  =  r  (18) 


The  kinematics  of  the  vehicle  is  described  by  Equations  (11)  and  (12)  where  Ucx  and  Ucy 
are  the  current  velocities  in  the  associated  direction.  The  steering  dynamics  of  ARIES  in 
matrix  form,  Mx  =  Ax  +  Bu,  is: 


1 

s 

1 

1 

o 

1 

1 

Yr-mUg 

0“ 

I 

I _ — 

Izz-N,  0 

r 

= 

N. 

0 

r 

+ 

N, 

0 

0  1 

0 

1 

0 

0 

(19) 


As  Johnson  (2001)  points  out,  5^(i)  is  a  generalized  command  that  represents  the 
control  input  to  both  rudders.  The  rudders  act  together,  but  turn  in  opposite  directions 
allowing  for  rapid  turning  during  operation. 
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The  final  assumption  made  for  ARIES  (Johnson,  2001)  is  that  the  eross  eoupling 
terms  in  the  mass  matrix  are  zero.  This  is  based  on  the  vehicle’s  symmetry  and  the 
rudders  being  very  close  to  being  equidistant  from  the  body  center.  Thus,  in  matrix  form, 
the  final  vehicle  dynamics  are  defined  as: 


1 

s 

1 

0 

0" 

1 

0“ 

1 

1 _ — 

0 

0 

r 

= 

N. 

0 

r 

-1- 

^^5 

0 

0 

1 

0 

1 

0 

0 

(20) 


Johnson  (2001)  determined  the  hydrodynamic  coefficients  for  ARIES  to  be  as 
follows: 

m  -  T,  =  456.76 
Y,  =69.90 
T,  =-68.16 
=  406.30 
N,  =-35.47 
A,  =-10.89 
A,  =-88.34 

B,  ARIES  CONTROL  LAWS  FOR  STEERING  AND  CROSS  TRACK 
ERROR 

This  section  describes  the  control  laws  used  by  ARIES  for  steering  and  cross 
track  error. 


ARIES  has  four  different  autopilots  for  flight  maneuvering  control:  diving, 
steering,  altitude  above  bottom,  and  cross  track  error.  With  the  object  of  accomplishing 
trajectory  planning,  only  the  steering  and  cross  track  error  controllers  are  of  interest.  The 
controllers  are  based  on  sliding  mode  control  theory  presented  by  Healey  and  Eienard 
(1993). 

Marco  and  Healey  (2001)  argue  that  a  second  order  model  is  sufficient  for 
heading  control.  The  sideslip  effects  are  treated  as  disturbances  that  the  control 
overcomes,  so  the  heading  model  becomes: 

r{t)  =  ar(t)  +  bb  ^(t)  +  (disturbances)  (21) 

tj/(0  =  r(0  (22) 
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From  past  in-water  experiments,  a  =  -0.30  sec'^  and  b  =  -0.1125  sec'^.  5,-(t)  is  the  stern 
rudder  angle.  The  sliding  surface  and  stern  rudder  command  for  heading  control  is  thus 
defined  by  Healey  and  Marco  (2001)  as: 

0(0  =  -0.9499r(0  +  0.170  l(v,„„  -  ¥(0)  (23) 

500  =  -1.543(2.539400 +  ritanh(o(0/4»))  (24) 

Where  r]=  1.0,  0.5  and  y/com-W(t)  is  the  heading  error. 

In  order  for  ARIES  to  follow  a  straight-line  path,  Marco  and  Healey  (2001)  use  a 
combination  of  line  of  sight  guidance  proposed  by  Healey  and  Lienard  (1993)  and  a  cross 
track  error  (CTE)  control.  The  reasoning  behind  this  is  that  with  large  heading  errors,  the 
cross  track  error  control  cannot  be  guaranteed  stable,  while  a  line  of  sight  heading  control 
will  reduce  heading  errors  to  zero.  Alternating  between  the  two  controllers  will  minimize 
both  cross  track  and  heading  errors.  Eigure  3  shows  the  track  geometry  and  velocity 
vector  diagrams  to  support  the  argument. 


Eigure  3.  Track  Geometry  and  Velocity  Vector  (Erom:  Marco  and  Healey,  2001) 

The  perpendicular  distance  between  the  center  of  the  vehicle  (V(t),T(t))  and  the 
adjacent  track  line  is  the  cross  track  error,  e(t).  The  goal  of  the  CTE  control  is  to 
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minimize  8(t).  The  total  track  length  is  defined  as  the  distance  between  the  ith  and  i-\ 
waypoints  and  is  given  by: 


The  ordered  pairs  {X„pt(i),  Y^ptfi))  and  (X„pt(i.]),  Y„pt(i.i))  are  the  current  and  previous  way 
points.  The  track  angle  is  defined  as: 

trk(i)  “  ^  wpt(i)  ~^wpt(i-\:))  (26) 

Where  arctan2  is  defined  by  MATLAB  as  the  inverse  tangent  function.  The  cross  track 
heading  error  for  the  ith  segment  is  defined  as: 

^iOcTE(i)  ~  trk(i)  (22) 

where  must  be  normalized  to  lie  between  ±180  degrees.  The  difference 

between  the  current  vehicle  position  and  the  next  way  point  is: 

^(0.mo=^.mo-^(0  (28) 

nO.,.(o=J;,KO-^(0  (29) 

With  the  above  definitions,  the  distance  to  the  ith  way  point  projected  to  the  track  line 
can  be  defined  as: 

^(0,=[^.,KO  4.(0 1  )]/T,  (30) 

therefore,  S(t)i  ranges  between  0-100  percent  of  Li.  The  cross  track  error  may  now  be 
defined  as: 

8(0  =  .S(0,sin(^/^(0)  (31) 

where  dp(t)  is  the  angle  (normalized  to  lie  between  ±180  degrees)  between  the  line  of 
sight  to  the  next  way  point  and  the  current  track  line  given  by: 

=  -  A„^,,,_i))-arctan2(f(0,,^,(,),i'(Owpoo)  (22) 
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Marco  and  Healey  (2001)  continue  by  defining  the  sliding  surfaee  in  terms  of 
derivatives  of  the  eross  traek  error  sueh  that  the  sliding  surfaee  for  the  CTE  eontroller 
becomes  a  second  order  polynomial  of  the  form; 

o(t)  —  t/r(t)cos(t|/(t)(-j.£(,))  +  +  A<2£(0  (33) 

The  rudder  input  is  thus  expressed  as: 

S,(0  =  (f/hcos(\i/(OCT£(o)"'(“^«''(Ocos(\j/(Oc7-£(o)  +  ^(''(0)"  sin(ti/(0c7-£(,))  (34) 

-  X^l7r(t)  cos(vj/(0CT£(,) )  -  ^2^  sin(¥(0c7-£(,) )  “  ^((^(O/ 4>) 

^71  ^71 

where  0  <  tff(Ocr£(o  ^  0-6,  ^^2  =  0.1,  p  =  0.1  and  (|)  =  0.5.  If  WiOcmo) 

ARIES  will  follow  the  traek,  but  travel  in  the  opposite  direction  to  that  desired.  In  order 
to  prevent  this  from  happening  in  praetiee,  a  bound  of  40  degrees  is  used  as  a  switeh  to 
line  of  sight  (EOS)  control. 

Marco  and  Healey  (2001)  determined  that  when  the  magnitude  of  the  cross  track 
heading  error  exeeeded  40  degrees,  a  EOS  eontroller  is  used  and  the  heading  eommand 
and  EOS  error  could  be  determined  from: 

V(0com(£OS)  =  arctan2(f(0„^,(,),^(Owpr(o)  (35) 

¥(0£05  =  W)co.(Los)  -  ¥(0  (36) 

The  control  laws  for  the  heading  controller.  Equations  23  and  24,  are  used  for  heading 
eontrol.  The  need  for  the  EOS  controller  is  apparent  in  two  cases:  1)  when  the  mission 
starts  and  ARIES’  initial  heading  is  greater  than  40  degrees  from  the  initial  way  point  and 
2)  when  the  angle  between  two  sequential  track  lines  exceed  40  degrees.  Once  the  eross 
track  heading  error  reduces  to  less  than  40  degrees,  ARIES  utilizes  the  CTE  controller. 

C.  SIMULATION  RESULTS 

Appendix  A  contains  the  MATLAB  .m  file  written  by  Marco  (2001)  that 
demonstrates  Mareo  and  Healey’s  heading  and  cross  track  error  controllers  for  the  NFS 
ARIES.  Table  1  shows  the  track. out  file  used  to  plan  the  simulated  ARIES  mission.  It 
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consists  of  5  tracks  (rows).  The  columns  are  defined  below  the  table.  Figure  4  shows  the 
results  of  the  simulation. 

Table  1.  track.out  file  (From:  Mareo  and  Healey,  2001) 


1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

10.0 

10.0 

2.75 

2.75 

0 

1.25 

1 

0 

25.00 

8.00 

40.00 

10.0 

210.0 

2.75 

2.75 

0 

1.25 

1 

0 

25.00 

8.00 

200.00 

40.0 

210.0 

2.75 

2.75 

0 

1.25 

1 

0 

25.00 

2.00 

30.00 

40.0 

10.0 

2.75 

2.75 

0 

1.25 

1 

0 

25.00 

2.00 

200.00 

-20.0 

-60.0 

2.75 

2.75 

0 

1.25 

1 

0 

25.00 

2.00 

100.00 

Column  #  Deseription 

1  X  position  way  point  (meters) 

2  Y  position  way  point  (meters) 

3  Left  serew  command  speed  (volts) 

4  Right  serew  command  speed  (volts) 

5  Control  mode  flag,  0  =  Depth  Control,  1  =  Altitude  Control 

6  Commanded  Altitude  (feet,  if  applieable) 

7  Commanded  Depth  (feet,  if  applicable) 

8  Perform  GPS  popup  on  this  traek?  1  =  Yes,  0  =  No 

9  Duration  of  GPS  popup  (seconds) 

10  Watch  Radius,  Rw(i)  (meters) 

11  Way  point  timeout,  (seconds) 
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60 


ARIES  Track  Actual  and  Planned 


Figure  4.  Simulation  Results  of  Heading  and  CTE  Controllers 


It  can  be  seen  from  Figure  4,  that  as  ARIES  starts  the  mission  at  (X=  -80,  7=10) 
with  an  initial  heading  of  50  degrees,  EOS  control  is  utilized  until  the  cross  track  heading 
error  falls  below  40  degrees  and  CTE  control  takes  over.  LOS  control  is  evident  at  each 
turn. 
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III.  TRAJECTORY  PLANNING 


A,  INTRODUCTION  AND  BACKGROUND 

As  previously  stated  in  Chapter  I,  trajeetory  traeking  is  defined  as  getting  from 
point  A  to  point  B  at  a  eertain  time.  Fraiehard  (1993)  eloquently  deseribed  trajeetory 
planning  of  a  robot  as  the  time  history  of  a  eontinuous  sequenee  of  eonligurations 
between  the  eurrent  eonliguration  of  the  robot  and  its  goal  configuration.  He  argued  that 
trajectory  planning  with  its  time  dimension  allows  accounting  of  time-dependent 
constraints  such  as  moving  obstacles  and  the  dynamic  constraints  of  the  robot.  Dynamic 
constraints  of  robots  are  usually  taken  to  be;  engine  force,  sliding  (friction  between  the 
wheels  and  ground),  and  velocity. 

With  the  object  of  defining  a  new  behavior  for  the  NFS  ARIES,  that  of  arriving  at 
a  defined  place  in  space  and  time,  this  chapter  will  seek  to  accomplish  the  object  by  first 
getting  ARIES  to  arrive  at  a  defined  place  in  space  and  then  second,  getting  ARIES  to 
arrive  at  a  defined  time. 

B.  ARRIVAL  AT  A  DEFINED  PLACE  IN  SPACE 

In  practice,  ARIES  will  be  used  to  develop  a  communications  server  vehicle. 
While  running  a  pre-programmed  mission,  she  will  be  expected  to  receive  modem 
commands  from  other  vehicles  that  will  direct  her  to  a  designated  rendezvous  location 
and  time  for  file  transfer  operations.  The  first  step  in  accomplishing  this  rendezvous 
requires  ARIES  to  respond  to  a  short-low  bit  rate  modem  command  and  then  go  to  the 
rendezvous  location  regardless  of  time.  This  was  accomplished  by  modifying  the 
MATLAB  .m  file  written  by  Marco  (2001). 

By  writing  a  break  into  the  .m  file,  thirty  seconds  into  the  simulation,  a  simulated 
modem  command  can  be  transmitted  to  ARIES  designating  a  rendezvous  position  in  the 
form  of  a  one-row  track.out  file  (Table  1).  The  new  track.out  file  essentially  overwrites 
the  original  track.out  file  and  the  simulation  proceeds  using  the  cross  track  error  (CTE) 
and  line  of  sight  (EOS)  controllers  explained  in  Chapter  II.  Eigure  5  shows  the  results  of 
sending  the  vehicle  to  rendezvous  location  of  (X=  -20,  Y=  100)  thirty  seconds  into  the 
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mission  of  Figure  4.  The  modifications  to  the  original  Marco  (2001)  MATLAB  .m  file 
can  be  seen  in  Appendix  B. 


ARIES  Track  Actual  and  Planned 


Figure  5:  Simulation  of  a  Modem  Command  Directing  ARIES  to  a  Rendezvous  Point 

C.  ARRIVAL  AT  A  DEFINED  PLACE  IN  TIME  WITHOUT 
ACCELERATION 

Having  previously  defined  S(t)i  as  the  distance  to  the  zth  way  point  projected  to 
the  track  line,  S(t)i  can  be  re-characterized  as  the  vehicle  path  and  s  can  simply  be  defined 
as  the  distance  traveled  along  the  path.  Velocity  can  now  be  defined  as  s  .  Using  theory 
developed  by  Fraichard  (1993),  the  state-time  space  of  ARIES  is  thus  a  three-dimensional 
space  {s  X  s  X  t),  where  t  represents  the  time  dimension.  The  dynamic  constraints  of 
ARIES  are  transformed  into  constraints  on  the  velocity,  s  ,  and  the  acceleration,  s  .  The 
constraints  on  s  translate  into  a  velocity  limit  curve  in  the  {s  x  s)  plane. 

With  only  a  top  speed  of  3.5  knots,  ARIES  is  not  capable  of  the  large  speed  changes  seen 
in  robots.  In  order  to  use  velocity  to  the  advantage  in  this  trajectory  planning  problem,  it 
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is  proposed  that  the  veloeity  limit  eurve  be  defined  as  a  step  funetion,  i.e.,  veloeity  is  one 
of  two  states:  maximum  (3.5  knots)  or  minimum  (0.5  knots).  Making  the  assumption  that 
the  aeeeleration  is  zero  further  simplifies  the  problem.  The  basie  uniform  motion 
equations:  s(t)  =  So  +  vt  and  v(t)  =  v  thus  hold  true. 

The  first  item  that  must  ehange  in  order  to  solve  the  rendezvous  problem  is  in 
regards  to  the  eleven-eolumn  traek.out  file  as  shown  in  Table  1.  In  its  eurrent 
configuration,  a  rendezvous  time  is  not  mentioned.  In  order  to  work  around  this,  at  the 
break  in  the  .m  file  that  allows  the  operator  to  send  a  simulated  modem  command,  a  one 
line,  twelve-column  traek.out  file  is  sent  instead  of  the  old  eleven-column  file.  The  new 
twelfth  column  indicates  the  time  to  rendezvous  in  seconds  after  the  modem  command. 

Now  that  ARIES  has  a  rendezvous  location  and  time,  the  mission  feasibility  must 
be  determined.  The  mission  feasibility  is  solely  based  on  the  velocity  constraints  of 
ARIES  (since  acceleration  is  assumed  to  be  zero).  If  the  time  allotted  for  the  rendezvous 
is  greater  than  the  distance  to  the  rendezvous  point  divided  by  the  maximum  speed  (3.5 
knots)  or  if  the  time  allotted  for  the  rendezvous  is  less  than  the  distance  to  the  rendezvous 
point  divided  by  the  minimum  speed  (0.5  knots),  the  mission  is  deemed  feasible  and  not 
constrained  by  the  velocity.  The  distance  to  the  rendezvous  point  (Ei)  is  defined  by 
equation  25. 

Once  the  mission  has  been  deemed  feasible,  the  simulation  proceeds  using  the 
pre-existing  CTE  and  EOS  controllers  to  determine  the  vehicle’s  relative  position  with 
regard  to  the  rendezvous  point.  A  simple,  speed  controller  is  then  implemented  to  control 
ARIES’  speed  with  respect  to  time.  This  is  accomplished  by  determining  the  overall 
distance  traveled  (ODTi)  in  a  given  time  step  as  defined  by: 

ODT,=L,_,-s^  (37) 

where  Si  is  the  distance  traveled  along  the  path.  The  time  used  is  simply  the  distance 
traveled  divided  by  ARIES’  speed,  and  the  time  remaining  until  rendezvous  is  the  given 
time  (column  twelve  of  the  traek.out  file)  minus  the  time  used. 

If  the  time  remaining  until  the  rendezvous  is  less  than  the  distance  traveled  along 
the  path  (5)  divided  by  the  speed  {U),  then  the  vehicle  is  sped  up  to  the  maximum  speed 
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of  3.5  knots.  If  the  time  remaining  is  greater  than  or  equal  to  — ,  then  ARIES  is  slowed 

to  0.5  knots.  Figure  6  is  a  three-dimensional,  time-spaee  plot  that  shows  the  results  of 
sending  ARIES  to  the  rendezvous  loeation  {X=  -20,  Y=  100,  t  =  90)  thirty  seeonds  into 
the  original  mission  depieted  in  Figure  4.  Having  assumed  no  acceleration.  Figure  7 
shows  the  step  function  behavior  of  ARIES’  speed. 


Time  Space  Plot 


Figure  6;  Three  Dimensional  Time-Space  Plot 
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Time  vs  Forward  Speed 


*  Original  Speed 
—  Rendezvous  Speed 

1 

0  20  40  60  80  100  120 

Time  (sec) 

Figure  7;  Time-Speed  Plot  Showing  Step  Behavior,  No  Acceleration/Deceleration 

This  simple  speed  controller,  that  assumes  uniform  motion,  solves  the  problem  of 
having  ARIES  arrive  at  a  defined  time.  The  assumption  of  no  acceleration,  however,  is 
flawed  and  is  now  addressed.  For  that  reason,  the  code  utilized  to  produce  Figures  6  and 
7  is  not  included  in  this  work. 

D.  ARRIVAL  AT  A  DEFINED  PLACE  IN  TIME  WITH  ACCELERATION 

Using  data  files  from  three  separate  missions  conducted  in  the  Azores  in  August 
2001,  the  longitudinal  ground  speed  was  plotted  against  the  time  in  order  to  determine 
ARIES’  acceleration  and  deceleration.  Figure  8  shows  the  data  from  mission  file 
Navd  081102  02.d. 
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Longitudinal  Ground  Speed  vs  Time  Units 


Figure  8;  Longitudinal  Ground  Speed  vs  Time  Units  for  Azores  Mission  081 10102 


For  this  mission,  it  was  estimated  that  ARIES  went  from  0  m/s  to  1.2  m/s  in 
approximately  856  time  units  where  each  time  unit  is  0.125  seconds.  This  is  an 
acceleration  rate  of  approximately  0.0112  m/s  .  The  other  two  mission  files 
(Navd_081101_04.d  and  Navd_081001_03.d)  showed  approximate  accelerations  of 
0.0152  m/s  and  0.0044  m/s  respectively.  These  results  led  to  an  estimate  of  ±0.01  m/s 
as  ARIES’  acceleration/deceleration  constant  for  the  sole  purpose  of  adding  acceleration 
into  the  previously  designed  speed  controller.  These  calculations  do  not  take  into 
consideration  that  ARIES  starts  on  the  surface  and  comes  to  its  ordered  depth. 

Now  that  acceleration  is  being  used,  acceleration  can  be  treated  as  a  dynamic 
constraint.  The  mission  cannot  be  feasible  if  the  distance  to  the  rendezvous  minus  the 
acceleration/deceleration  distance  is  less  than  the  min/max  speed  multiplied  by  the  time  it 
takes  to  accelerate/decelerate  from  initial  speed  (U)  to  final  speed  (Uo).  The 

acceleration/deceleration  distance  is  given  by: 
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(38) 


d  = 


where  a  is  the  aeeeleration  eonstant. 


Onee  the  mission  is  determined  to  be  feasible,  if  ARIES  needs  to  inerease  her 
speed  based  on  the  distanee  remaining  to  the  rendezvous  point  and  time  remaining  until 
the  rendezvous  time,  speed  is  adjusted  as: 


U, 


'  U  ■  ,  Q  t  j  ■ 

/-I  I  used, I 


(39) 


where  Ui  is  the  new  speed,  Uui  is  the  previous  speed,  at  is  the  aeeeleration  eonstant  and 
tused,i  is  the  time  used  between  (i-1)  and  i.  The  maximum  Ui  is  held  at  3.5  knots.  If 
ARIES  needs  to  deerease  her  speed,  the  following  equation  is  used: 


U,  =  U, 


at  j  ■ 

i  used,i 


(40) 


The  minimum  speed  of  ARIES  is  held  to  0.5  knots. 

Eigure  9  shows  the  effeet  of  deeeleration  on  the  time-speed  plot  for  the  mission 
shown  in  Eigure  6. 
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Forward  Speed  (m/s) 


Time  vs  Forward  Speed 


Figure  9;  Time-Speed  Plot  With  Deeeleration 
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IV.  BUILDING  AN  ALONG  TRACK  SPEED  (ATS)  CONTROLLER 


A.  THEORY  BEHIND  THE  LONGITUDINAL  EQUATION  OF  MOTION 

The  use  of  the  basic  uniform  motion  equations:  s(t)  =  Sg  +  vt  and  v(t)  =  v  with  the 
addition  of  acceleration  does  not  hold  true  for  an  underwater  vehicle.  For  that  reason,  the 
code  utilized  to  produce  Figure  9  is  not  included  in  this  work.  Chapter  II  discussed  the 
equations  of  motion  for  the  ARIES  vehicle.  Healey  (1995)  further  derives  the  surge 
equation  of  motion  in  order  to  model  the  longitudinal  dynamics  of  the  vehicle.  He 
assumes  that  the  hydrodynamic  forces  in  the  surge  direction  are  not  generated  by  lift  and 
are  dominated  by  drag  and  added  mass  effects.  He  also  points  out  that  the  primary  thrust 
forces  for  ARIES  comes  from  the  propellers.  This  gives  rise  to  the  equation  of  motion 
for  longitudinal  motion: 

{m  -  )u^  =  mv^r  +  A„  |  +  (41) 

Xprop  represents  the  net  propulsive  force  on  the  vehicle  from  propeller  action.  It  consists 
of  two  parts:  the  bollard  pull  force  and  the  loss  of  thrust  caused  by  the  subsequent 
forward  motion  of  the  vehicle.  The  bollard  pull  force  is  equal  to  an\n\  where  a  equals 

(KtpD^^).  Kt  is  the  thrust  coefficient  and  is  a  function  of  the  propeller  speed  of  advance. 
The  loss  of  thrust  caused  by  the  subsequent  forward  motion  of  the  vehicle  is  equal  to 
Yn\u\  where  y  equals  y^pD^ .  The  coefficient  yo  is  the  slope  of  the  Kt  curve  at  the 

particular  operating  condition  of  interest.  Assuming  the  lateral,  sideslip  velocity  is 
negligible  simplifies  equation  41  yielding  the  equation  of  motion  for  longitudinal  motion 
in  a  straight  line  as: 

(m  -  )u^  =  |m J  +  an  |n|  -  7|n|  (42) 

From  Johnson  (2001),  the  value  of  m  for  ARIES  is  222.26  kg,  the  length  (a)  is  64 
inches  and  the  diameter  (b)  is  7.53  inches.  Based  on  work  by  Lamb  (1945),  for  a 
neutrally  buoyant  mass  and  equating  the  total  volume  of  ARIES  to  a  prolate  ellipsoid 
model  while  maintaining  the  overall  length  results  in  an  a/b  ratio  of  8.499.  Interpolating 
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Lamb’s  table  results  in  a  kx  value  of  0.0266  which  results  va  =  6.79  kg  and  (m  -  ) 

=  215.47  kg.  Solving  for  results  in; 

=  0.00464 1(X^  y  |Mj  +  an|n|-  (43) 

B,  PARAMETER  IDENTIFICATION 

The  unknown  values  in  equation  43  are  X^y^,  a,  and  y.  In  order  to  successfully 

identify  the  unknown  coefficients  it  is  common  practice  to  accurately  model  the  ARIES 
control  and  response.  By  manipulating  the  equation  of  motion,  we  can  let  system 
parameters  become  unknowns  and  the  variables,  as  measured,  to  be  known.  Data 
gathered  by  ARIES  from  her  onboard  sensors  is  sampled  at  eight  Hertz  and  is  therefore 
not  continuous.  This  calls  for  the  use  of  a  discrete  time  model.  In  this  case, 

u^{t)  -  where  At  equals  0.125  seconds.  This  simplifies  equation  43  to: 

At 

(t  + 1)  -  (t)  =  0.00464 lAt(X^  1^  (t) \u^ (t)|  +  an{t) \n{t)\  -  y\n{t)\ (t))  (44) 

Equation  44  can  be  cast  into  matrix  form:  y(t)  =  H(t)0(t)  where  y(t)  = 
+  ,  H(t)  is  the  n  X  3  matrix  of  measurements  relating  to  the  output  (i.e.  n(t) 

and  Ur(t))  and  0(t)  is  the  3  x  1  matrix  of  coefficients  {X^y^,a,  and  y). 

As  Johnson  (2001)  pointed  out,  using  a  least  squares  method  to  estimate  the 
parameters  0(t),  which  are  never  exactly  known,  results  in  minimizing  the  difference 
between  the  actual  parameter  and  its  estimate.  The  difference  is  known  as  the  equation 
error  and  it  is  defined  as: 

e{t)  =  {y{t)-H{t)m)  (45) 

where  0(t)  is  the  estimate  of  Oft).  Eor  this  particular  case,  it  involves  using  the  data 
obtained  from  the  vehicle  sensors  combined  with  the  equation  44.  Johnson  (2001)  further 
points  out  that  in  order  to  minimize  the  error,  define  the  scalar  positive  squared  error 

n 

measure,  J{n)  =  '^e{t)e{t)  then  the  minimization  of  Jis  given  by: 

t=\ 
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(46) 


dJ 

d@ 


0  =  -Y,H\t)e{t) 

7=1 


and  substituting  =  H(t)0(t)  produces: 


()  =  -Y,H\t){y{t)-H{tmt))  (47) 

7=1 


Rearranging  this  into  matrix  form  and  solving  for  0(7)  gives: 

@  =  [H'HY^H'y  (48) 

Using  the  matrix  divide  function  in  MATLAB  to  evaluate  equation  48  will  produce  a 
result  that  is  the  least  squares  fit  for  0(7) . 

Gelb  (1974)  points  out  that  the  same  result  may  be  found  using  the  gaussian 
random  assumptions  in  which  the  solution,  0(7) ,  in  equation  (47)  is  the  most  likely 
solution  in  which  its  probability  is  maximum.  It  should  be  noted  that  the  regression 

n 

matrix,  [E  H  (7)77(7)] ,  must  be  positive  and  strong  (with  no  singularity)  otherwise  its 

7=1 

inverse  does  not  exist.  This  means  that  the  system  must  be  perpetually  excited  by  its 
input. 

C.  DEFINING  THE  COEFFICIENTS  FOR  THE  LONGITUDINAL 
EQUATION  OF  MOTION 


In  order  to  gather  the  required  output  data  (n(t)  and  u/tj)  to  solve  equation  44  by 
the  method  of  least  squares,  an  experiment  was  planned.  ARIES  was  programmed  with 
the  track,  out  file  listed  in  Table  2. 

Table  2.  track.out  file  for  17  April  2002  Experiment 


1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

800.00 

300.00 

3.25 

3.25 

0 

8.0 

2.0 

1 

25.0 

10.00 

40.0 

900.00 

300.00 

3.25 

3.25 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1000.00 

300.00 

2.50 

2.50 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1100.00 

300.00 

3.25 

3.25 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1200.00 

300.00 

2.50 

2.50 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1300.00 

300.00 

3.25 

3.25 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1300.00 

350.00 

3.25 

3.25 

0 

8.0 

2.0 

0 

25.0 

10.00 

75.0 

1200.00 

350.00 

3.25 

3.25 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

25 


1100.00 

350.00 

2.50 

2.50 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1000.00 

350.00 

3.25 

3.25 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

900.00 

350.00 

2.50 

2.50 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

800.00 

350.00 

3.25 

3.25 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

The  values  in  the  eolumns  are  described  as  follows: 

Column  #  Description 

1  X  position  way  point  (meters) 

2  Y  position  way  point  (meters) 

3  Left  screw  command  speed  (volts) 

4  Right  screw  command  speed  (volts) 

5  Control  mode  flag,  0  =  Depth  Control,  1  =  Altitude  Control 

6  Commanded  Altitude  (feet,  if  applicable) 

7  Commanded  Depth  (feet,  if  applicable) 

8  Perform  GPS  popup  on  this  track?  1  =  Yes,  0  =  No 

9  Duration  of  GPS  popup  (seconds) 

10  Watch  Radius,  Rw(i)  (meters) 

1 1  Way  point  timeout,  (seconds) 


The  mission  described  in  Table  2  starts  ARIES  out  at  a  speed  of  approximately 
1.8  m/s.  After  conducting  a  GPS  popup,  the  vehicle  runs  for  approximately  100  meters  at 
this  speed  and  then  the  speed  is  reduced  to  approximately  1.4  m/s  for  the  next  100  meters. 
ARIES  is  then  accelerated  to  1.8  m/s  for  the  next  100  meters.  This  pattern  is  continued 
until  reaching  the  turn  around  point.  Upon  turning  around,  the  vehicle  continues  the 
speed  increase/decrease  pattern  until  the  end  point.  In  this  configuration,  four  sets  of 
acceleration/deceleration  data  can  be  obtained. 

The  experiment  was  successfully  conducted  in  Monterey  Bay  on  17  April  2002. 
In  total,  three  data  runs  were  conducted.  In  the  third  run,  the  right  and  left  screw 
command  voltages  were  slightly  modified  due  to  the  first  two  runs  showing  that  the 
voltages  were  not  matched.  The  track,  out  file  used  in  run  three  is  shown  in  Table  3. 


Table  3.  track.out  file  for  Run  3 


1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

800.00 

300.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

40.0 

900.00 

300.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1000.00 

300.00 

2.40 

2.40 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1100.00 

300.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1200.00 

300.00 

2.40 

2.40 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1300.00 

300.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1300.00 

350.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

75.0 
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1200.00 

350.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1100.00 

350.00 

2.50 

2.50 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

1000.00 

350.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

900.00 

350.00 

2.50 

2.50 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

800.00 

350.00 

2.8 

2.8 

0 

8.0 

2.0 

0 

25.0 

10.00 

150.0 

Figure  10  shows  the  planned  and  actual  tracks  and  Figure  1 1  shows  the  changes  in 
longitudinal  speed  during  the  three  runs. 


Actual  and  Planned  Track  Run  1 


Y  (meters) 


Actual  and  Planned  Track  Run  2 


Figure  10;  Actual  and  Planned  Tracks  of  17  April  2002  Experiment 
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Vehicle  Speed  Run  1 
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Vehicle  Speed  Run  2 


Figure  11;  Speed  Plots  for  17  April  2002  Experiment 


With  the  data  gathered  from  this  experiment,  the  MATLAB  file  shown  in 
Appendix  C,  coefficients. m,  was  utilized  to  determine  the  unknown  coefficients,  |„  | , 

a,  and  y.  It  should  be  noted  that  during  this  process  it  was  discovered  that  previous  work 
had  identified  the  propeller  rotation  rate  in  rotations  per  minute  (rpm)  was  calculated  by 
multiplying  the  input  voltage  by  a  factor.  The  factor  for  the  port  propeller  is  133.8047 
and  for  the  starboard  propeller  the  factor  is  124.4615.  Looking  at  the  technical 
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specifications  provided  by  the  manufactor,  Tecnadyne  Corporation,  a  factor  of  266 
should  be  used  to  determine  the  number  of  rpm  for  the  propellers.  Using  the  266  factor 
for  determining  propeller  speed  based  on  input  voltage,  coefficients. m  determined  that 
1^^  I  =  -15.681,  a=  0.079,  and  y=  1.247.  Plugging  these  values  into  equation  44  and 

using  the  propeller  speed  data  obtained  from  the  experiment  the  accuracy  of  the 
coefficients  could  be  determined.  Figure  12  shows  the  results  of  the  model  in  relation  to 
the  actual  longitudinal  speed. 


Vehicle  Speed  Run  1 


Figure  12;  Model  and  Actual  Vehicle  Speed 
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D,  COEFFICIENT  IDENTIFICATION  FROM  THE  RATIONAL  APPROACH 

As  can  be  seen  from  Figure  12,  the  coefficients  generated  by  the  MATLAB  file, 
eoeffieients.m,  do  not  aeeurately  model  the  data.  Assuming  ARIES  has  a  drag  eoeffieient 

2 

(Cd)  of  0.1,  an  area  of  0.103  m  and  the  density  of  seawater  (p)  is  1025  — then  , 
ean  be  ealeulated  using  the  formula: 

=  ^PC^A  (49) 

The  result  is  A  ,  ,  ~  -5.25.  This  value  of  A  ,  ,  translates  into  a  foree  of  approximately 

16.35  N  (3.68  Ibf)  when  the  vehicle  is  traveling  at  1.76  m/s.  This  results  in  a  typieal  Kt 
value  for  AUV/ROV  propellers  of  about  0.4.  This  value  of  Kt  requires  the  value  of  a  = 
0.0793.  Lastly  by  determining  that  the  thrust  reduetion  faetor  was  the  determining 
eoeffieient  in  the  model,  y=  0.05  was  ehosen.  Plugging  these  new  values  {X^y^  =  - 

5.25,  a  =  0.0793,  and  y=  0.05)  into  equation  44  and  using  the  propeller  speed  data 
obtained  from  the  experiment  the  aceuracy  of  these  new  eoeffieients  eould  be 
determined.  Figure  13  shows  the  results  of  the  model  in  relation  to  the  actual 
longitudinal  speed. 
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Vehicle  Speed  Run  1 


Figure  13.  New  Model  and  Actual  Vehicle  Speed 
Although  the  results  shown  in  Figure  13  look  accurate  for  predicting  the  speed  of 
ARIES,  giving  further  thought  to  the  value  chosen  for  the  drag  coefficient,  Cd,  it  was 
determined  that  with  the  various  fins  on  the  body  of  ARIES,  a  Cd  =  0. 1  was  too  small  and 
a  Cd  =  0.2  was  more  appropriate.  This  change  results  in  |  ~  -10.5.  This  value  of 

y  I  translates  into  a  force  of  approximately  32.7  N  (7.36  Ibf)  when  the  vehicle  is 

traveling  at  1.76  m/s.  This  changes  the  value  for  Kj  to  0.86,  which  is  closer  to  the  value 
provided  by  Tecnadye  of  about  1.16.  This  value  of  Kj  requires  the  value  of  a  =0.155. 
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The  y=  0.05  was  not  changed.  Plugging  these  new  values  |  =  -10.5,  a  =  0.155, 

and  Y=  0.05)  into  equation  44  and  using  the  propeller  speed  data  obtained  from  the 
experiment  the  accuracy  of  these  new  coefficients  could  be  determined.  Figure  14  shows 
the  results  of  the  model  in  relation  to  the  actual  longitudinal  speed. 


Vehicle  Speed  Run  1 


Figure  14:  Best  Model  and  Actual  Vehicle  Speed 
It  can  be  seen  from  Figure  14,  that  this  model  is  more  accurate  than  the  previous 
model  in  predicting  the  actual  speed  measured  by  ARIES  for  these  three  data  runs.  The 
difficulty  in  obtaining  the  coefficients  for  the  longitudinal  equation  of  motion  can  best  be 
explained  by  the  very  small  operating  range  of  the  experiment.  The  speed  range  was  at 
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the  most,  approximately  0.5  m/s.  This  translates  into  a  very  small  region  on  the  standard 
Thrust  and  Torque  Coefficients  versus  Angle  of  Attack  chart  (Lewis,  1988).  Final 
coefficients  are  summarized  in  Table  4  shown  below: 

Table  4.  Summary  of  Coefficient  Values 


^  1 

w,. 

-10.5 

a 

0.155 

7 

0.05 

E,  THE  SPEED  CONTROLLER 

With  the  coefficients  for  the  longitudinal  equation  of  motion  identified,  a  sliding 
mode  controller  was  developed  based  on  the  work  of  Healey  and  Lienard  (1993). 
Rewriting  equation  44  with  the  coefficients  identified  yields: 

M  AO  =  0.00464  lAd-10.5MAOK  (0|  +  0. 155«(0|«(0|  -  0.05|«(0|m,  (0)  (50) 

Considering  the  -0.05|«(0|MAOas  a  disturbance,  equation  50  can  be  rewritten  as: 

u^{t)  =  0.00464  1AA-10.5m^(0|w^(0|  +  0.155n(O|«(O|)  (51) 

Choosing  =  the  sliding  surface,  where  Ucom  is  the  desired  vehicle 

speed,  the  control  law  in  terms  of  the  command  for  n(t),  propeller  speed,  is  found  from: 

d(0  =  -r\  tanh(o(0  /  (|))  (52) 

This  results  in  the  control  law  being  defined  as: 

n(t)  =  39(u^^^  (0  -  r\  tanh(o  /  (|)))  +  67.74m  (0|  (53) 

The  propeller-speed  command  thus  arises  from  one  term  to  accelerate  the  vehicle,  one 
term  to  stabilize  the  motion  and  one  term  to  overcome  the  vehicle  forward  drag.  If  a 
nonzero  acceleration  is  required,  is  used,  otherwise  for  conditions  where  it  is 

required  to  ‘regulate’  speed,  is  held  to  zero. 
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V. 


RESULTS 


A,  DIFFERENT  TIME  COMBINATIONS  AT  SAME  [X,Y]  POSITION 

The  control  law,  equation  53,  and  the  longitudinal  equation  of  motion,  equation 
50,  were  inserted  in  the  MATLAB  file  called  finalrendezvous.m  (Appendix  D)  in  place 
of  the  pre-existing,  rudimentary  speed  controller.  Choosing  a^=0.1,a7  =  800  and  a 
rendezvous  position  of  [-20,  100]  at  a  time  of  70  seconds  produced  Figures  15,  16  and  17. 
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ARIES  Track  Actual  and  Planned 


- Initial  Track  ARIES 

-  Track  After  Modem  Command  ARIES 
0  Rendezvous  Point 

-  REMUS  Track 

—  Planned  Track  ARIES 


Figure  15:  X-Y  Plot  for  [-20,  100]  Rendezvous  Location 
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Time  Space  Plot 


Figure  16:  3-D  Plot  for  70  Second  Rendezvous  Time 
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Time  vs  Forward  speed  Propeller  Speed 


Figure  17:  Speed  vs  Time  for  70  Second  Rendezvous  Time  and  Propeller  Speed  vs  Time 
The  above  figures  show  that  thirty  seconds  into  ARIES’  mission,  the  REMUS 
requests  a  rendezvous  at  location  [-20,  100]  in  70  seconds  and  ARIES  makes  the 
rendezvous.  The  speed  controller  commands  ARIES  to  speed  up  from  her  initial  cruising 
speed  of  1.4  m/s  in  order  to  achieve  the  rendezvous  at  the  designated  time.  The  propeller 
speed  (shown  in  the  plot  to  the  right)  adjusts  as  necessary. 

Keeping  the  same  rendezvous  location  of  [-20,  100]  but  changing  the  rendezvous 
time  to  90  seconds  and  120  seconds  results  in  Figures  18  through  21. 
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Time  Space  Plot 


Figure  18:  3-D  Plot  for  90  Second  Rendezvous  Time 
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Time  vs  Forward  Speed 


Figure  19:  Speed  vs  Time  for  90  Seeond  Rendezvous  Time 
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Time  Space  Plot 


Figure  20:  3-D  Plot  for  120  Second  Rendezvous  Time 
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Figure  21;  Speed  vs  Time  for  120  Seeond  Rendezvous  Time 
The  above  four  figures  show  that  by  increasing  the  rendezvous  time  by  twenty 
seconds,  the  speed  controller  responds  by  commanding  ARIES  to  slow  down  in  order  to 
reach  the  rendezvous  location  at  the  designated  time. 

B.  SAME  TIME  COMBINATIONS  AT  DIFFERENT  [X,Y]  POSITION 

By  holding  the  time  of  rendezvous  constant  at  120  seconds  and  changing  the 
rendezvous  position  from  [-20,  100]  to  [-100,  150]  and  [50,  50],  Figures  22  through  27 
were  produced. 
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Figure  22:  X-Y  Plot  for  [-100,  150]  Rendezvous  Location 
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Time  Space  Plot 
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Figure  23:  3-D  Plot  for  [-100,  150]  Rendezvous  Location 
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Time  vs  Forward  Speed 


Figure  24:  Speed  vs  Time  for  [-100,  150]  Rendezvous  Location 
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Figure  25:  X-Y  Plot  for  [50,  50]  Rendezvous  Location 
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Time  Space  Plot 


Figure  26:  3-D  Plot  for  [50,  50]  Rendezvous  Location 
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Figure  27:  Speed  vs  Time  for  [50,  50]  Rendezvous  Loeation 
In  both  cases,  the  speed  controller  reacts  to  the  change  in  location  and  commands 
the  vehicle  to  the  proper  speed  in  order  to  arrive  at  the  designated  time. 
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VI.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

This  work  has  demonstrated  that  the  ARIES  AUV  can  be  outfitted  with  a  new 
behavior  of  arriving  at  a  defined  place  in  space  and  time  and  that  the  pre-existing  cross¬ 
track  error  guidance  algorithms  can  be  extended  to  close  along  track  as  well  as  cross 
track  errors  resulting  in  a  trajectory  controller. 

While  only  a  simulation,  this  work  can  be  extremely  useful  to  the  mission 
planner.  The  ability  to  determine  ARIES’  response  over  various  distances  will  allow  the 
MIW  Commander  to  effectively  plan  the  use  of  his  assets  when  setting  up  search  patterns 
for  the  REMUS  vehicles  and  waiting  patterns  for  ARIES. 

B,  RECOMMENDATIONS 

The  difficulty  encountered  in  determining  the  hydrodynamic  coefficients  of  the 
longitudinal  equation  of  motion  can  be  directly  attributed  to  the  size  and  number  of  open 
ocean  missions  that  were  conducted.  In  order  to  fully  realize  these  values,  more  runs 
would  need  to  be  conducted.  It  is  further  recommended  that  each  run  start  at  ARIES’ 
minimum  speed  and  then  incrementally  increase  her  speed  in  small  intervals  until 
reaching  maximum  speed.  Stepping  ARIES  speed  back  down  to  minimum  in  the  same 
interval  would  enhance  the  data  for  her  deceleration.  Overall  this  data  would  be  a  vast 
improvement  over  the  data  that  was  collected  during  the  17  April  2002  experiment  and 
would  increase  confidence  in  the  model  for  all  speed  scenarios. 

The  simulation  presented  involved  ARIES  receiving  a  simulated  command  from 
REMUS  to  rendezvous  at  a  particular  place  and  time.  It  was  shown  that  ARIES  is 
constrained  by  her  velocity  and  acceleration  and  therefore  will  not  always  be  able  to 
conform  to  REMUS’  command.  There  is  no  method  in  place  for  ARIES  to  make  this 
known  to  REMUS  and  propose  an  alternate  location  and/or  time.  Additionally,  the 
simulation  is  hard  coded  to  ask  the  user  to  input  a  simulated  modem  command  thirty 
seconds  into  ARIES’  mission.  In  practice,  ARIES  should  be  able  to  receive  a  command 
to  rendezvous  at  any  time  during  her  mission.  It  is  obvious  that  further  study  involving 
the  communication  between  vehicles  is  thus  warranted. 
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Finally,  while  this  simulation  shows  that  ARIES  can  achieve  a  rendezvous  in 
space  and  time,  it  is  only  a  simulation.  Transforming  the  code  contained  in 
finalrendezvous.m  (Appendix  D)  into  ARIES’  C  programming  language  would  be  a 
worthy  endeavor,  as  the  Center  for  AUV  Research  stands  ready  to  receive  their  fourth 
generation  vehicle  in  the  coming  months.  With  two  vehicles  in  operation,  the  work 
presented  here  can  someday  be  validated  on  the  open  ocean. 
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APPENDIX  A.  MATLAB  FILE  WAYPOINT.M 

This  appendix  contains  a  MATLAB  file  written  by  Marco  (2001)  that  simulates 
ARIES  using  heading  control  and  cross  track  error  control  for  a  planned  path. 

%File  CTE  Loiter 
whitebg('k'); 

%  State  =  [v  r  psi] 
clear 

TRUE  =  1; 

FALSE  =  0; 

DegRad  =  pi/180; 

RadDeg  =  180/pi; 

%  State  Model  PArameters 
W  =600.0; 

U=  1.4*3.28; 
g  =  32.174; 

Boy  =  500.0; 
xg  =0.125/12.0; 
m  =  W/g; 

rho  =  1.9903; 

L=  10; 

Iz  =  (l/12)*m*(1.33^2  +  10^2);  %  Approx.  Using  I  =  l/12*m*(a^2  +  b^2) 

Iz  =  Iz*5.0; 

Yv_dot  =  -0.03430*(rho/2)*L^3; 

Yrdot  =  -0.00178*(rho/2)*LM; 

Yv  =  -0.10700*(rho/2)*L^2; 

Yr  =  0.01187*(rho/2)*L^3; 

Ydrs  =  (0.01241*(rho/2)*L^2)/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 
Ydrb  =  (0.01241*(rho/2)*L^2)/2.0; 

Nv_dot  =  -0.00178*(rho/2)*LM; 

%Nr_dot  =  -0.00047*(rho/2)*L^5; 

Nrdot  =  -Iz; 

Nv  =  -0.00769*(rho/2)*L^3; 

Nr  =  -0.00390*(rho/2)*L^4; 

%Ndrs  =  -2.6496/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 
%Ndrb  =  1.989/2.0; 

%  Below  Modified  on  7/12/00  The  3.5  and  3.4167  is  the  Moment  Arm  Length  in  Feet 
Ndrs  =  -0.01241*(rho/2)*(L''2)*(3.5)/2.0;  %  Since  Stem  Lower  Rudder  Removed 
Ndrb  =  0.01241*(rho/2)*(L''2)*(3.4167)/2.0;  %  Since  Bow  Lower  Rudder  Removed 

%  Combining  Stem  &  Bow  Rudder  Effectivness 
Ndr  =  Ndrs  -  Ndrb; 

Ydr  =  Ydrs  -  Ydrb;  %  Cancel  Out 
ml  =  m  -  Yv  dot; 
m2  =  m*xg  -  Yr  dot; 
m3  =  m*xg  -  Nv  dot; 
m4  =  Iz  -  Nr  dot; 

Y1  =  Yv; 

Y2  =  Yr; 
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Y3  =  U^2*Ydr; 

N1  =Nv; 

N2  =  Nr; 

N3  =  U^2*Ndr; 

A  =  [Y1*U  Y2*U;N1*U  N2*U]; 

B  =  [Y3  N3]'; 

M  =  [ml  m2;m3  m4]; 

A1  =  inv(M)*A; 

B1  =iiiv(M)*B; 

AO  =  [Al(l,l)  Al(l,2)0; 

A1(2,1)A1(2,2)0; 

0  10]; 

BO  =  [B1;0]; 

dt  =  0.125; 
t  =  [0:dt:1000]'; 

size(t) 

%  set  initial  conditions 

start=10; 

v(l)  =0.0; 

r(l)  =0.0; 

rRM(l)=r(l); 

%  This  is  the  Initial  Heading  of  the  Vehicle 
psi(l)  =  50.0*DegRad; 

%  This  is  the  Initial  Position  of  the  Vehicle 
X(l)  =  -80.0;  %  Meters 
Y(l)=  10.0; 

%  Convert  to  Feet 

%  this  data  from  track.out  file 

No_tracks=5; 

Track=[  10.0  10.0  2.75  2.75  0  1.25  1.00  0  25.00  8.00  40.00 
10.0  210.0  2.75  2.75  0  1.25  1.00  0  25.00  8.00  200.00 

40.0  210.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  30.00 

40.0  10.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  200.00 

-20.0  -60.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  100.00]; 

track=Track(:,  1 :2); 

SurfaceTime  =  Track(:,9); 

SurfPhase  =  Track(:,8); 

%  readin  wayopoints  from  track  data  assumes  track  is  loaded 
for  j=l  :No_tracks, 

X_Way_c(j)  =track(j,l); 

Y_Way_c(j)  =  track(j,2); 
end; 

%Set  start  position 
PrevX_Way_c(l)  =  -80.0; 

PrevY_Way_c(l)  =  10.0; 
room  =  0.0; 

W_R=  10.0; 
a  =  -.3; 
b  =  (9/24)*a; 
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x(:,l)  =  [v(l);r(l);psi(l)]; 

%  Below  are  in  British  Units  for  CTE  Sliding  Mode 
%Laml  =  0.75; 

%Lam2  =  0.5; 

Laml  =  2.0; 

Lam2  =  1.0; 

EtaFlightHeading  =1.0; 

PhiFlightHeading  =  0.5; 

%  Below  for  tanh 
Eta_CTE  =  0.1; 

EtaCTEMin  =  1.0; 

PhiCTE  =  0.5; 

Uc  =  []; 

Vc  =  []; 

PLOTPART  =  0; 

SegLen(  1 )  =  sqrt((X_Way_c(  1  )-PrevX_Way_c(  1  ))*2+(Y_Way_c(  1  )-PrevY_Way_c(  1  ))^2); 
psi_track(  1 )  =  atan2(Y_Way_c(  1  )-PrevY_Way_c(  1  ),X_Way_c(  1  )-PrevX_Way_c(  1 )); 

for  j =2 :  N  otracks , 

SegLen(j)  =  sqrt((X_Way_c(j)-X_Way_c(j-l))''2+(Y_Way_c(j)-Y_Way_c(j-l))''2); 
p  si_track(j )  =  atan2  ( Y_  W  ay_c  (j )  -  Y_W  ay_c  (j  - 1 )  ,X_  W  ay_c  (j )  -X_  W  ay_c(j  - 1 )) ; 
end; 

Sigma  =  []; 

Depthcom  =  []; 
dr=[]; 
drl  =  []; 
drl(l)  =  0.0; 

Depth_com(l)  =  5.0; 

WayPointVertDist  com  =  [5.0  5.0  5.0  5.0  5.0]; 

SURFACETIMERACTIVE  =  FALSE; 
for  i=l:length(t)-l, 

Depth_com(i)  =  WayPoinfVertDist_com(j); 

X_Way_Error(i)  =  X_Way_c(j)  -  X(i); 

Y_Way_Error(i)  =  Y_Way_c(j)  -  Y(i); 

%  DeWrap  psi  to  within  +/-  2.0*pi; 
psi_cont(i)  =  psi(i); 
while(abs(psi_cont(i))  >  2.0*pi) 
psi_cont(i)  =  psi_cont(i)  -  sign(psi_cont(i))*2.0*pi; 
end; 

psi_errorCTE(i)  =  psi_cont(i)  -  psi_track(j); 

%  DeWrap  psi  error  to  within  +/-  pi; 
while(abs(psi_errorCTE(i))  >  pi) 

psi_errorCTE(i)  =  psi_errorCTE(i)  -  sign(psi_errorCTE(i))*2.0*pi; 
end; 

%  **  Always  Calculate  this 
Beta  =  v(i)/U; 

%  Beta  =  0.0; 

cpsie  =  cos(psi_errorCTE(i)+Beta); 
spsie  =  sin(psi_errorCTE(i)+Beta); 
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s(i)  =  [X_Way_Error(i),Y_Way_Error(i)]*... 

[(X_Way_c(j)-PrevX_Way_c(j)),(Y_Way_c(j)-PrevY_Way_c(j))]'; 

%  s  is  distance  to  go  projected  to  track  line(goes  from  0-100%L) 

s(i)  =  s(i)/SegLen(j); 

Ratio=(1.0-s(i)/SegLenG))*  100.0; 

%  ** 

%  radial  distance  to  go  to  next  WP 

ss(i)  =  sqrt(X_Way_Error(i)^2  +  Y_Way_Error(i)^2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line 
dp(i)  =  ... 

atan2(  (Y_Way_c(j)-PrevY_Way_c(j)),(X_Way_c(j)-PrevX_Way_c(j)) )... 

-  atan2(  Y_Way_Error(i),X_Way_Error(i) ); 
if(dp(i)>pi), 

dp(i)  =  dp(i)  -  2.0*pi; 
end; 

cte(i)  =  s(i)*sin(dp(i)); 

if(  abs(psi_errorCTE(i))  >=  40.0*pi/180.0  |  s(i)  <  0.0  ), 

%  Use  LOS  Control 
LOS(i)  =  1; 

psicomLOS  =  atan2(Y_Way_Error(i),X_Way_Error(i)); 
psi_errorLOS(i)  =  psi  comLOS  -  psi_cont(i); 
if(abs(psi_errorLOS(i))  >  pi), 
psi_errorLOS(i)  =  ... 

psi_errorLOS(i)  -  2.0*pi*psi_errorLOS(i)/abs(psi_errorLOS(i)); 
end; 

Sigma  FlightHeading  =  0.9499*(r_com  -  r(i))  +  0.1701*psi_errorLOS(i); 
dr(i)  =  -1.5435*(2.5394*r(i) ... 

+  Eta_FlightHeading*tanh(Sigma_FlightHeading/Phi_FlightHeading)); 

else 

%  Use  CTE  Controller 
LOS(i)  =  0; 

if(cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 

Sigma(i)  =  U*rRM(i)*cpsi_e  +  Laml*U*spsi_e  +  3.28*Lam2*cte(i); 
dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*rRM(i)*cpsi_e  +  U*rRM(i)^2*spsi_e  ... 

-  Laml*U*rRM(i)*cpsi_e  -  Lam2*U*spsi_e  -  Eta_CTE*(Sigma(i)/Phi_CTE)); 

else 

dr(i)  =  dr(i-l); 

end; 

end;  %  End  of  CTE  Controller 

%  use  LOS  if  near  to  loiter  point 

%  if  (loiter==l)&  s(i)<10;  dr(i)=drlos(i);end; 

%  Surface  Phase  Logic  (Independent  of  LOS  or  CTE) 

if(SurfPhaseG)  ==  TRUE) 
if(SURFACE_TIMER_ACTIVE  —  FALSE) 
if(Ratio  >  40.0) 

%  Start  a  Timer 

SURFACETIMERACTIVE  =  TRUE; 

Depth_com(i)  =  0.0; 

SurfaceWait  =  SurfaceTimeG)  +  t(i); 
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SurfaceWait 

end; 

end; 

end; 

if(SURFACE_TIMER_ACTIVE  ==  TRUE) 
if(t(i)  >=  SurfaceWait) 

SURFACETIMERACTIVE  =  FALSE; 

Depth_com(i)  =  WayPointVertDist_com(j); 

SurfPhase(j)  =  0; 
else 

Depth_com(i)  =  0.0; 
end; 
end; 

if(abs(dr(i))  >  0.4) 
dr(i)  =  0.4*sign(dr(i)); 
end; 

%  Model  drl  is  the  actual  lagged  rudder,  dr  is  the  rudder  command. 
%  taudr  =  0.255; 

%  drl(i+l)  =  drl(i)  +  dt*(dr(i)-drl(i))/taudr; 

%  if(abs(drl(i))  >  0.4) 

%  drl(i)  =  0.4*sign(drl(i)); 

%  end; 


%Jay  Johnson  Model; 

Yv  =-68.16; 

Yr  =406.3; 

Ydr  =  70.0; 

Nv  =-10.89; 

Nr  =-88.34; 

Ndr  =  -35.47; 

MY  =  456.76; 

IN  =  215; 

M  =  diag([MY,IN,l]); 

AA  =  [Yv,Yr,0;Nv,Nr,0;0,l,0]; 

BB  =  [Ydr;Ndr;0]; 

A  =  inv(M)*AA; 

B  =inv(M)*BB; 

%  x_dot(:,i-l-l)  =  [  A(l,l)*v(i)  -l-  A(l,2)*r(i)  -I-  B(l)*drl(i); 
%  A(2,l)*v(i)  A(2,2)*r(i)  B(2)*drl(i); 

%  r(i)]; 

x_dot(:,i-l-l)  =  [  A(l,l)*v(i)  -l-  A(l,2)*r(i)  -I-  B(l)*dr(i); 
A(2,l)*v(i)  A(2,2)*r(i)  B(2)*dr(i); 

r(i)]; 

x(:,i-i-l)  =  x(:,i)-i-dt*x_dot(:,i); 
v(Ul)  =x(l,Ul)/12; 
r(i+l)  =x(2,i+l); 
psi(Ul)  =  x(3,Ul); 
rRM(Ul)  =  r(Ul); 
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%  Added 

%  rRM(i+l)  =  rRM(i)  +  dt*(a*rRM(i)  +  b*dr(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  Throw  in  some  Waves 
%Uc(i)  =  -0.5*sin(2*pi*t(i)/5); 

%Vc(i)  =  0.5*sin(2*pi*t(i)/5); 

%Model  using  system  ID  results  from  Bay  tests 

%  rRM(i+l)  =  rRM(i)  +  dt*(a*rRM(i)  +  b*drl(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  side  slip  added  proprtional  to  turn  rate  from  AZORES  data  V  in  ft/sec 
%  v(i+l)  =  1.0*rRM(i+l)*3.28; 

Uc  =  0.0; 

Vc  =  0.0; 

%Kinematics 

X(i+1)  =  X(i)  +  (Uc  +  (U/3.28)*cos(psi(i))  -  v(i)/3.28*sin(psi(i))  )*dt; 
Y(i+1)  =  Y(i)  +  (Vc  +  (U/3.28)*sin(psi(i))  +  v(i)/3.28*cos(psi(i))  )*dt; 

%  Check  to  See  if  we  are  Within  the  Watch  Radius 

if(sqrt(X_Way_Error(i)^2.0  +  Y_Way_Error(i)^2.0)  <=  W_R  |  s(i)  <  0.0), 
disp(sprintf('WayPoint  %d  Reached',))); 
i  f(j  ==No_tracks) , 

PLOTPART  =  1; 
break; 
end; 

PrevX_Way_c(j+l)  =  X_Way_c(j); 

PrevY_Way_c(j+l)  =  Y_Way_c(j); 

j=j+i; 

end; 

end; 

dr(i+l)  =  dr(i); 
cte(i+l)  =  cte(i); 
s(i+l)  =  s(i); 
ss(i+l)  =  ss(i); 

if(PLOT_PART), 

figure(l); 

plot(t([l:i+l]),psi*  180/pi); 
hold; 

plot(t([  1  :i+ 1  ]),dr*  1 80/pi,'r');grid; 

hold;zoom  on; 

figure(2); 

plot(t([l:i+l]),cte); 

hold; 

plot(t([l:i+l]),s,'r'); 
plot(t([l:i+l]),ss,'g');grid; 
hold;zoom  on; 
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else 

figure(l); 
plot(t,psi*  180/pi); 
hold; 

plot(t,dr*l  80/pi, 'r'); 

hold;grid; 

figure(2); 

plot(t,cte); 

hold; 

plot(t,s,'r'); 
plot(t,ss,'g');grid; 
hold;zoom  on; 
end; 

figure(3); 

plot(Y,X);grid; 

hold 

plot([Y_Way_c(l)PrevY_Way_c(l)],[X_Way_c(l)PrevX_Way_c(l)],'r'); 
for  ii=2:No_tracks, 

plot([ Y_W ay_c(ii)  Y_Way_c(ii- 1 )] ,  [X_Way_c(ii)  X_W ay_c(ii- 1 )] ,  'r') ; 
end; 

hold;zoom  on; 
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APPENDIX  B.  MATLAB  FILE  RENDEZVOUS.M 


This  appendix  contains  a  MATLAB  file  based  on  the  file  in  appendix  A  that 
simulates  ARIES  receiving  a  modem  command  thirty  seconds  into  her  planned  mission 
for  her  to  rendezvous  at  a  specific  location. 

%  This  file  has  a  pre-defined  track.out  file  in  it.  30  seeonds  into  the 
%  simulation,  the  user  is  asked  if  ARIES  should  be  diverted  to  a 
%  rendezvous  point.  If  the  answer  is  'yes',  the  user  is  prompted  to  enter 
%  a  new  traek.out  file  that  eonsists  of  one  row  (This  simulates  an  aeoustie 
%  modem  command).  The  simulation  will  show  ARIES  aborts  the  planned  track 
%  and  goes  to  the  directed  rendezvous  point.  If  the  answer  is  'no'  to  the 
%  query  concerning  the  rendezvous  point,  the  mission  continues  as  planned. 

%  All  equation  numbers  (i.e,  Eq  (#))  refer  to  equations  presented  in  Marco 
%  Healey’s  “Command,  Control  and  Navigation  Experimental  Results 
%  With  the  NFS  ARIES  AUV” 

%  06  Feb  2002 

whitebg('k'); 

%  State  =  [v  r  psi] 
clear 

TRUE  =  1; 

FALSE  =  0; 

%  Converts  Degrees  to  Radians  &  Radians  to  Degrees 

DegRad  =  pi/180; 

RadDeg  =  180/pi; 

%  State  Model  Parameters 

W  =  600.0;  %  Weight  in  LB 

U  =  1.4*3.28;  %  Forward  Speed  ? 

g  =  32.174;  %  Gravity  in  ft/sec^2 

Boy  =  500.0;  %  Bouyancy  ? 

xg  =0.125/12.0;  %  ?? 

m  =  W/g;  %  Mass 

rho  =  1.9903;  %  Density  of  Seawater  in  slugs/ft^3 

L  =  10;  %  Length  in  ft  of  ARIES 

Iz  =  (l/12)*m*(1.33^2  +  10^2);  %  Approx.  Using  I  =  l/12*m*(a^2  +  b''2) 

%  where  a  is  width  &  b  is  length 

Iz  =  Iz*5.0; 

%  Coefficients 

Yv  dot  =  -0.03430*(rho/2)*L^3;  %  Added  Mass  in  Sway  Coefficient. 

Yr  dot  =  -0.00178*(rho/2)*L^4;  %  Added  Mass  in  Yaw  Coefficient. 

Yv  =  -0. 10700*(rho/2)*L^2;  %  Coeff  of  Sway  Force  induced  by  Side  Slip 

Yr  =  0.01 187*(rho/2)*L^3;  %  Coeff  of  Sway  Force  induced  by  Yaw 

Ydrs  =  (0.01241*(rho/2)*L^2)/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 
Ydrb  =  (0.01241*(rho/2)*L^2)/2.0;  %  So  don't  use  these  equations 

Nv  dot  =  -0.00178*(rho/2)*L^4;  %  Added  Mass  Moment  of  Inertia  in  Sway  Coeff 
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%Nr_dot  =  -0.00047*(rho/2)*L^5; 

Nr  dot  =  -Iz;  %  Added  Mass  Moment  of  Inertia  in  Yaw  Coeff 

Nv  =  -0.00769*(rho/2)*L^3;  %  Coeff  of  Sway  Moment  from  Side  Slip 

Nr  =  -0.00390*(rho/2)*LM;  %  Coeff  of  Sway  Moment  from  Yaw 

%Ndrs  =  -2.6496/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 

%Ndrb  =  1.989/2.0; 

%  Below  Modified  on  7/12/00  The  3.5  and  3.4167  is  the  Moment  Arm  Length 
%  in  Feet  -  Since  Bow  &  Stem  Lower  Rudders  Removed 

Ndrs  =  -0.01241*(rho/2)*(L^2)*(3.5)/2.0; 

Ndrb  =  0.01241*(rho/2)*(L^2)*(3.4167)/2.0; 

%  Combining  Stem  &  Bow  Rudder  Effectivness 

Ndr  =  Ndrs  -  Ndrb; 

Ydr  =  Ydrs  -  Ydrb;  %  Cancel  Out 

%  Matrices 

ml  =  m  -  Yv  dot; 
m2  =  m*xg  -  Yr  dot; 
m3  =  m*xg  -  Nv  dot; 
m4  =  Iz  -  Nr  dot; 

Y1  =  Yv; 

Y2  =  Yr; 

Y3  =  U^2*Ydr; 

N1  =Nv; 

N2  =  Nr; 

N3  =  U^2*Ndr; 

A  =  [Y1*U  Y2*U;N1*U  N2*U]; 

B  =  [Y3  N3]'; 

M  =  [ml  m2;m3  m4]; 

A1  =  inv(M)*A; 

B1  =inv(M)*B; 

AO  =  [Al(l,l)  Al(l,2)0; 

A1(2,1)A1(2,2)0; 

0  10]; 

BO  =  [B1;0]; 
dt  =  0.125; 
t  =  [0:dt:1000]'; 
size(t); 

%  Set  initial  conditions 

start=10; 
v(l)  =0.0; 
r(l)  =0.0; 
rRM(l)=r(l); 
psi(l)  =  50.0*DegRad; 

X(l)  =  -80.0; 

Y(l)=  10.0; 

%  Convert  to  Feet  ? 

%  This  data  from  track.out  file 


%  Initial  Side  Slip  Velocity 
%  Initial  Yaw 

%  Initial  Heading  of  ARIES 
%  Initial  Position  in  Feet 
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No_tracks=5;  %  Sets  #  of  Tracks  =  #  of  Rows 

Track=[  10.0  10.0  2.75  2.75  0  1.25  1.00  0  25.00  8.00  40.00 
10.0  210.0  2.75  2.75  0  1.25  1.00  0  25.00  8.00  200.00 

40.0  210.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  30.00 

40.0  10.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  200.00 

-20.0  -60.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  100.00]; 

track=Track(:,l  :2);  %  Defines  track  as  Track(X,Y) 

SurfaceTime  =  Track(:,9);  %  Col  9  of  Track  is  Surface  Time  for  Pop-up 

SurfPhase  =Track(:,8);  %  Col  8  of  Track  designates  if  Pop-up 

%  Read  in  wayopoints  from  track  data  assumes  track  is  loaded 

for  j=l  :No_tracks, 

X_Way_c(j)  =track(j,l); 

Y_Way_c(j)  =track(j,2); 
end; 

%  Set  start  position 

PrevX_Way_c(l)  =  -80.0; 

PrevY_Way_c(l)  =  10.0; 
rcom  =  0.0; 

W_R  =10.0;  %  Sets  initial  Watch  Radius 

a  =  -.3; 

b  =  (9/24)*a; 

x(:,l)  =  [v(l);r(l);psi(l)]; 

%  Below  are  in  British  Units  for  CTE  Sliding  Mode 
%Laml  =  0.75; 

%Lam2  =  0.5; 

Laml  =  2.0; 

Lam2  =  1.0; 

EtaFlightHeading  =1.0; 

PhiFlightHeading  =  0.5; 

%  Below  for  tanh 

Eta_CTE  =  0.1; 

EtaCTEMin  =  1.0; 

PhiCTE  =  0.5; 

Uc  =  []; 

Vc  =  []; 

PLOT  PART  =  0;  disp(sprintf('PLOT_PART  =  0')); 

%  Total  Track  Length  between  initial  waypoint  and  waypoint  (1) 

SegLen(l)  =  sqrt((X_Way_c(l)-PrevX_Way_c(l))^2-l-(Y_Way_c(l)... 
-PrevY_Way_c(l))^2); 

%  Track  Angle  of  first  track 

psi_track(  1 )  =  atan2(Y_Way_c(  1  )-PrevY_Way_c(  1  ),X_Way_c(  1  )-PrevX_Way_c(  1 )); 
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%  Computes  track  lengths  and  track  angles  for  each  track 
for  j =2 :  N  otracks , 

SegLen(j)  =  sqrt((X_Way_c(j)-X_Way_c(j-l))^2+(Y_Way_c(j)-... 
Y_Way_cG-l))^2); 

p  si_trackG )  =  atan2  ( Y_  W  ay_c  G )  -  Y_W  ay_c  G  - 1 )  ,X_  W  ay_c  G )  -X_  W  ay_cG  - 1 )) ; 
end; 

Sigma  =  []; 

Depthcom  =  []; 
dr=[]; 
drl  =  []; 
drl(l)  =  0.0; 

Depth_com(l)  =  5.0; 

WayPointVertDist  com  =  [5.0  5.0  5.0  5.0  5.0]; 

SURFACETIMERACTIVE  =  FALSE; 

%  Starts  a  loop  that  computes  values  for  each  data  point  corresponding  to 
%  a  time  value  (in  this  case,  every  0.125  seconds  from  1  to  1000  seconds) 

for  i=l:length(t)-l, 

Depth_com(i)  =  WayPointVertDistcomG); 

%  Difference  between  current  vehicle  position  &  the  next  waypoint  Eq(13) 

X_Way_Error(i)  =  X  Way  cG)  -  X(i); 

Y_Way_Error(i)  =  Y_Way_cG)  -  Y(i); 

%  DeWrap  psi  to  within  +/-  2.0*pi;  Makes  Pleading  Angle  to  he  between 
%  0-360  degrees 

psi_cont(i)  =  psi(i); 
while(abs(psi_cont(i))  >  2.0*pi) 
psi_cont(i)  =  psi_cont(i)  -  sign(psi_cont(i))*2.0*pi; 
end; 

%  Cross  Track  Pleading  Error  Eq(12) 

psi_errorCTE(i)  =  psi_cont(i)  -  psi  trackG); 

%  DeWrap  psi  error  to  within  +!-  pi;  Normalized  to  Lie  between  +!-  180 
%  degrees 

while(abs(psi_errorCTE(i))  >  pi) 

psi_errorCTE(i)  =  psi_errorCTE(i)  -  sign(psi_errorCTE(i))*2.0*pi; 
end; 

%  **  Always  Calculate  this  (What  is  This?) 

Beta  =  v(i)/U; 

%  Beta  =  0.0; 

cpsie  =  cos(psi_errorCTE(i)-l-Beta); 
spsie  =  sin(psi_errorCTE(i)-l-Beta); 

%  Distance  to  the  ith  way  point  projected  to  the  track  line  S(t)i  - 
%  Eq  (14) 
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s(i)  =  [X_Way_Error(i),Y_Way_Error(i)]*[(X_Way_c(j)-... 
PrevX_Way_c(j)),(Y_Way_c(j)-PrevY_Way_c(j))]'; 

%  s  is  distance  to  go  projected  to  track  line(goes  from  0-100%L)  -  Eq  (14) 

s(i)  =  s(i)/SegLen(j); 

Ratio=(1.0-s(i)/SegLen(j))*  100.0;  %  Ranges  from  0-100%  of  SegLen 

%  Radial  distance  to  go  to  next  WP 

ss(i)  =  sqrt(X_Way_Error(i)^2  -I-  Y_Way_Error(i)^2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line  -  Eq  (16) 

dp(i)  =  atan2(  (Y_Way_c(j)-PrevY_Way_c(j)),(X_Way_c(j)-... 

PrevX_Way_c(j)) )-  atan2(  Y_Way_Error(i),X_Way_Error(i) ); 
if(dp(i)>pi), 

dp(i)  =  dp(i)  -  2.0*pi; 
end; 

%  Cross  Track  Error  Definition  -  Eq  (15) 
cte(i)  =  s(i)*sin(dp(i)); 

%  If  the  magnitude  of  the  CTE  Heading  exceeds  40  degrees,  a  LOS  Controller 
%  is  used. 

if(  abs(psi_errorCTE(i))  >=  40.0*pi/180.0  |  s(i)  <  0.0  ), 

LOS(i)=  1; 

psi  comLOS  =  atan2(Y_Way_Error(i),X_Way_Error(i));  %  Eq  (22) 
psi_errorLOS(i)  =  psi  comLOS  -  psi_cont(i);  %  Eq  (23) 

%  LOS  Error 

if(abs(psi_errorLOS(i))  >  pi), 

psi_errorLOS(i)  =  psi_errorLOS(i)  -  2.0*pi*psi_errorLOS(i)... 
/abs(psi_errorLOS(i)); 

end; 

%  Eq  (8) 

Sigma  FlightHeading  =  0.9499*(r_com  -  r(i))  -I-  0.1701*psi_errorLOS(i); 
%  Eq  (9) 

dr(i)  =  -1.5435*(  2.5394*r(i)-l-  Eta_FlightHeading*tanh... 
(SigmaFlightHeading/PhiFlightHeading)); 

else 

%  Use  CTE  Controller  if  CTE  Heading  is  less  than  40  degrees 
LOS(i)  =  0; 

if(cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 

%  Sliding  Surface  -  Eq  (20) 
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Sigma(i)  =  U*rRM(i)*cpsi_e  +  Laml*U*spsi_e  +  3.28*Lam2*cte(i); 
%  Rudder  Input  -  Eq  (21) 

dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*rRM(i)*cpsi_e  +  U*rRM(i)^2*... 
spsi  e  -  Laml*U*rRM(i)*cpsi_e  -  Lam2*U*spsi_e  -  Eta  CTE*  ... 
(Sigma(i)/Phi_CTE)); 

else 

dr(i)  =  dr(i-l); 

end; 

end;  %  End  of  CTE  Controller 

%  Use  LOS  if  near  to  loiter  point 
%  if  (loiter==l)&  s(i)<10;  dr(i)=drlos(i);end; 

%  Surfaee  Phase  Logie  (Independent  of  LOS  or  CTE) 

if(SurfPhase(j)  —  TRUE) 
if(SURFACE_TIMER_ACTIVE  ==  FALSE) 
if(Ratio  >40.0) 

%  Start  a  Timer 

SURFACETIMERACTIVE  =  TRUE; 

Depth_eom(i)  =  0.0; 

SurfaeeWait  =  SurfaeeTime(j)  +  t(i); 

Surfaee  Wait 
end; 
end; 
end; 

if(SURFACE_TIMER_ACTIVE  ==  TRUE) 
if(t(i)  >=  SurfaeeWait) 

SURFACETIMERACTIVE  =  FALSE; 

Depth_eom(i)  =  WayPointVertDist_eom(j); 

SurfPhase(j)  =  0; 
else 

Depth_eom(i)  =  0.0; 

end; 

end; 

if(abs(dr(i))  >  0.4) 
dr(i)  =  0.4*sign(dr(i)); 
end; 

%  Model  drl  is  the  aetual  lagged  rudder,  dr  is  the  rudder  eommand. 

%  taudr  =  0.255; 

%  drl(i+l)  =  drl(i)  +  dt*(dr(i)-drl(i))/taudr; 

%  if(abs(drl(i))  >  0.4) 

%  drl(i)  =  0.4*sign(drl(i)); 

%  end; 

%  Jay  Johnson  Model 

Yv  =-68.16; 

Yr  =406.3; 

Ydr  =  70.0; 

Nv  =-10.89; 
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Nr  =-88.34; 

Ndr  =  -35.47; 

MY  =  456.76; 

IN  =  215; 

M  =  diag([MY,IN,l]); 

AA  =  [Yv,Yr,0;Nv,Nr,0;0,l,0]; 

BB  =  [Ydr;Ndr;0]; 

A  =  iiiv(M)*AA; 

B  =iiiv(M)*BB; 

%  x_dot(:,i-l-l)  =  [  A(l,l)*v(i)  +  A(l,2)*r(i)  -I-  B(l)*drl(i); 

%  A(2,l)*v(i)  +  A(2,2)*r(i)  B(2)*drl(i); 

%  r(i)]; 

x_dot(:,i+l)  =  [  A(l,l)*v(i)  +  A(l,2)*r(i)  -I-  B(l)*dr(i); 

A(2,l)*v(i)  +  A(2,2)*r(i)  B(2)*dr(i); 

r(i)]; 

x(:,i+l)  =  x(:,i)-i-dt*x_dot(:,i); 
v(i+l)  =x(l,i-M)/12; 
r(i+l)  =x(2,i+l); 
psi(i+l)  =  x(3,i+l); 
rRM(i-M)  =  r(i-H); 

%  Added 

%  rRM(i-l-l)  =  rRM(i)  -I-  dt*(a*rRM(i)  -I-  b*dr(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  Throw  in  some  Waves 
%  Ue(i)  =  -0.5*sin(2*pi*t(i)/5); 

%  Ve(i)  =  0.5*sin(2*pi*t(i)/5); 

%  Model  using  system  ID  results  from  Bay  tests 

%  rRM(i-l-l)  =  rRM(i)  -I-  dt*(a*rRM(i)  -I-  b*drl(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  side  slip  added  proprtional  to  turn  rate  from  AZORES  data  V  in  ft/see 
%  v(i+l)  =  1.0*rRM(i-M)*3.28; 

Ue  =  0.0; 

Ve  =  0.0; 

%  Kinematies 

X(i-l-l)  =  X(i)  -I-  (Ue  +  (U/3.28)*eos(psi(i))  -  v(i)/3.28*sin(psi(i))  )*dt; 
Y(i-l-l)  =  Y(i)  -I-  (Ve  +  (U/3.28)*sin(psi(i))  +  v(i)/3.28*eos(psi(i))  )*dt; 

%  This  should  abort  @30  seeonds  if  input  is  empty  or  'Y' 

%  This  modifieation  done  on  05  Feb  2002  -  original  file  is  waypointl.m 

ifi==240, 

K  =  input('Is  Rendezvous  Required?  (Enter  1  for  Yes,  0  for  No)— »'); 
if  isempty(K)==l;  K  =  1;  break;  end; 
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if  K  ==  1;  break; 
else  i  =  i;  end; 
end 


%  Check  to  See  if  we  are  Within  the  Watch  Radius 

if(sqrt(X_Way_Error(i)^2.0  +  Y_Way_Error(i)^2.0)  <=  W_R  |  s(i)  <  0.0), 
disp(sprintf('WayPoint  %d  Reached',))); 
i  f(j  ==No_tracks) , 

PLOTPART  =  1; 
disp(sprintf('PLOT_PART  =  1')); 
break; 
end; 

PrevX_Way_c(j+l)  =  X_Way_c(j); 

PrevY_Way_c(j+l)  =  Y_Way_c(j); 

j=j+i; 

end; 

end; 

%  Requests  Rendezvous  Point  Information 

%  This  modification  done  on  05  Feb  2002  -  original  file  waypoint2.m 

ifj  ==No_tracks, 
disp(sprintf('Mission  Complete')); 
else 

newrcom  =  0.0; 
new_v(l)  =  v(i+l); 
new_r(l)  =  r(i+l); 
new_rRM(l)  =  new_r(l); 
new_psi(l)  =  psi(i+l); 

New_X(l)=X(i+l); 

New_Y(l)  =  Y(i+1); 

New_No_Tracks  =  1; 

New  Track  =  input('Enter  1 1  Column  Track,  i.e.,  [1  1  ...]-»'); 

newtrack  =  New_Track(:,l  :2); 

newSurfaceTime  =  New_Track(:,9); 

newSurfphase  =  New_Track(:,8); 

for  jj  =  l:New_No_Tracks, 

New_X_Way_c(jj)  =  new_track(jj,l); 

New_Y_Way_c(jj)  =  new_track(jj,2); 
end; 

New_PrevX_Way_c(l)  =  X(i+1);  %  Sets  Abort  Posit  as  start  of  new 

New_PrevY_Way_c(l)  =  Y(i+1);  %  track. 

%  Below  for  tanh 
newEtaCTE  =  0.1; 
newEtaCTEMin  =  1.0; 
newPhiCTE  =  0.5; 

PLOT  PART  =  0;  disp(sprintf('PLOT_PART  =  0')); 
new_x(:,l)  =  [new_v(l);  new_r(l);  new_psi(l)]; 

%  Total  Track  Length  between  abort  point  and  rendezvous  point 
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New_SegLen(l)  =  sqrt((New_X_Way_c(l)-New_PrevX_Way_c(l))^2+.. 
(New_Y_Way_c(l)  -  New_PrevY_Way_c(l))^2); 

%  Track  Angle  of  track  between  abort  point  and  rendezvous  point 

new_psi_track(l)  =  atan2(New_Y_Way_c(l)-New_PrevY_Way_c(l),... 
N  ew_X_  W  ay_c  ( 1 )  -N  e  w_Pre  vX_  W  ay_c(  1 )) ; 

%  Starts  loop  that  computes  values  for  each  data  point  corresponding  to  a 
%  time  value  along  new  track 

newSigma  =  []; 
newDepthcom  =  []; 
new_dr=[]; 
newdrl  =  []; 
new_drl(l)  =  0.0; 
new_Depth_com(l)  =  5.0; 

new  WayPointVertDist  com  =  [5.0  5.0  5.0  5.0  5.0]; 
new_SURFACE_TIMER_ACTIVE  =  FALSE; 
tt  =  [t(i+l):dt:500]'; 
size(tt); 

for  ii  =  l:length(tt)-l, 

new_Depth_com(ii)  =  new_WayPointVertDist_com(jj); 
New_X_Way_Error(ii)  =  New_X_Way_c(jj)  -  New_X(ii); 
New_Y_Way_Error(ii)  =  New_Y_Way_c(jj)  -  New_Y(ii); 
new_psi_cont(ii)  =  new_psi(ii); 
while(abs(new_psi_cont(ii))  >  2.  0*pi) 
new_psi_cont(ii)  =  new_psi_cont(ii)  -  sign(new_psi_cont(ii))*... 

2.0*pi; 

end; 

%  Cross  Track  Pleading  Error  Eq(12) 

new_psi_errorCTE(ii)  =  new_psi_cont(ii)  -  new_psi_track(jj); 

%  DeWrap  psi  error  to  within  +/-  pi;  Normalized  to  Lie  between  +/-  180 
%  degrees 

while(abs(new_psi_errorCTE(ii))  >  pi) 
new_psi_errorCTE(ii)  =  new_psi_errorCTE(ii)  -  sign(... 
new_psi_errorCTE(ii))  *2 . 0  *pi; 

end; 

%  **  Always  Calculate  this  (What  is  This?) 

newBeta  =  new_v(ii)/U; 

%  Beta  =  0.0; 

newcpsie  =  cos(new_psi_errorCTE(ii)+new_Beta); 
newspsie  =  sin(new_psi_errorCTE(ii)+new_Beta); 

%  Distance  to  the  ith  way  point  projected  to  the  track  line  S(t)i  - 
%  Eq  (14) 

new_s(ii)  =  [New_X_Way_Error(ii),New_Y_Way_Error(ii)]*[(... 

N  ew_X_W  ay_c(j  j  )-N  ew_Pre  vX_W  ay_c(j  j )) ,  (N  e  w_  Y_  W  ay_c  (j  j ) . . 
-New_PrevY_Way_c(jj))]'; 
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%  s  is  distance  to  go  projected  to  track  line(goes  from  0-100%L)  -  Eq  (14) 

new_s(ii)  =  new_s(ii)/New_SegLen(jj); 

%  Ranges  from  0-100%  of  SegLen 
Ratio=(1.0-new_s(ii)/New_SegLen(jj))*  100.0; 

%  Radial  distance  to  go  to  next  WP 

new_ss(ii)  =  sqrt(New_X_Way_Error(ii)^2  -I-  New_Y_Way_Error(ii)^2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line  -  Eq  (16) 

new_dp(ii)  =  atan2(  (New_Y_Way_c(jj)-New_PrevY_Way_c(jj)),(-. 
New_X_Way_c(jj)-New_PrevX_Way_c(jj)) )-  atan2... 

(N e w_ Y_ W ay_Error(ii)  ,N  ew_X_ W ay_Error(ii) ) ; 
if(new_dp(ii)  >  pi), 

new_dp(ii)  =  new_dp(ii)  -  2.0*pi; 
end; 

%  Cross  Track  Error  Definition  -  Eq  (15) 

new_cte(ii)  =  new_s(ii)*sin(new_dp(ii)); 

%  If  the  magnitude  of  the  CTE  Heading  exceeds  40  degrees,  a  LOS  Controller 
%  is  used. 

if(  abs(new_psi_errorCTE(ii))  >=  40.0*pi/180.0  |  new_s(ii)  <  0.0  ), 
new_LOS(ii)  =  1 ; 

new_psi_comLOS  =  atan2(New_Y_Way_Error(ii),New_X_Way_Error(ii)); 
new_psi_errorLOS(ii)  =  new_psi_comLOS  -  new_psi_cont(ii); 
if(abs(new_psi_errorLOS(ii))  >  pi), 
new_psi_errorLOS(ii)  =  new_psi_errorLOS(ii)  -  2.0*pi*... 
new_psi_errorLOS(ii)/abs(new_psi_errorLOS(ii)); 
end; 

%  Eq  (8) 

new  Sigma  FlightHeading  =  0.9499*(new_r_com  -  new_r(ii))  -t... 
0.1701*new_psi_errorLOS(ii); 

%  Eq  (9) 

new_dr(ii)  =  -1.5435*(  2.5394*new_r(ii)-l-  Eta_FlightHeading*tanh... 
(new_Sigma_FlightHeading/Phi_FhghtHeading)); 

else 

%  Use  CTE  Controller  if  CTE  Heading  is  less  than  40  degrees 
new_LOS(ii)  =  0; 

if(new_cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 

%  Sliding  Surface  -  Eq  (20) 
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new_Sigma(ii)  =  U*new_rRM(ii)*new_cpsi_e  +  Laml*U*new_spsi_e 
+  3.28*Lam2*new_cte(ii); 

%  Rudder  Input  -  Eq  (21) 

new_dr(ii)  =  (1.0/(U*b*new_cpsi_e))*(-U*a*new_rRM(ii)*... 
new  cpsi  e  +  U*new_rRM(ii)^2*new_spsi_e  -  Laml*U*... 
new_rRM(ii)*new_cpsi_e  -  Lam2*U*new_spsi_e  -  new  Eta  CTE* 
(new_Sigma(ii)/new_Phi_CTE)); 
else 

new_dr(ii)  =  new_dr(ii-l); 

end; 

end;  %  End  of  CTE  Controller 

%  Use  LOS  if  near  to  loiter  point 

%  if  (loiter==l)&  new_s(ii)<10;  new_dr(ii)=new_drlos(ii);end; 

%  Surface  Phase  Logic  (Independent  of  LOS  or  CTE) 
if(SurfPhase  ==  TRUE) 

if(new_SURFACE_TIMER_ACTIVE  ==  FALSE) 
if(Ratio  >40.0) 

%  Start  a  Timer 

new_SURFACE_TIMER_ACTIVE  =  TRUE; 
new_Depth_com(ii)  =  0.0; 
newSurfaceWait  =  new_SurfaceTime(l)  +  tt(ii); 
newSurfaceWait 
end; 
end; 
end; 

if(new_SURFACE_TIMER_ACTIVE  ==  TRUE) 
if(tt(ii)  >=  new  SurfaceWait) 
new_SURFACE_TIMER_ACTIVE  =  FALSE; 
new_Depth_com(ii)  =  new_WayPointVertDist_com(l); 
new_SurfPhase(l)  =  0; 
else 

new_Depth_com(ii)  =  0.0; 

end; 

end; 

if(abs(new_dr(ii))  >  0.4) 
new_dr(ii)  =  0.4*sign(new_dr(ii)); 
end; 

%  Model  drl  is  the  actual  lagged  rudder,  dr  is  the  rudder  command. 

%  taudr  =  0.255; 

%  drl(i+l)  =  drl(i)  +  dt*(dr(i)-drl(i))/taudr; 

%  if(abs(drl(i))  >  0.4) 

%  drl(i)  =  0.4*sign(drl(i)); 

%  end; 

new_x_dot(:,ii+l)  =  [  A(l,l)*new_v(ii)  +  A(l,2)*new_r(ii)  +  B(l)*... 
new_dr(ii);  A(2,l)*new_v(ii)  +  A(2,2)*new_r(ii)  +  B(2)*... 
new_dr(ii);  new_r(ii)]; 

new_x(:,ii+l)  =  new_x(:,ii)+dt*new_x_dot(:,ii); 
new_v(ii+l)  =  new_x(l,ii+l)/12; 
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new_r(ii+l)  =  new_x(2,ii+l); 
new_psi(ii+l)  =  new_x(3,ii+l); 
new_rRM(ii+l)  =  new_r(ii+l); 

%  Added 

%  rRM(i+l)  =  rRM(i)  +  dt*(a*rRM(i)  +  b*dr(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  Throw  in  some  Waves 
%  Uc(i)  =  -0.5*sin(2*pi*t(i)/5); 

%  Vc(i)  =  0.5*sin(2*pi*t(i)/5); 

%  Model  using  system  ID  results  from  Bay  tests 

%  rRM(i+l)  =  rRM(i)  +  dt*(a*rRM(i)  +  b*drl(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  side  slip  added  proprtional  to  turn  rate  from  AZORES  data  V  in  ft/sec 
%  v(i+l)  =  1.0*rRM(i+l)*3.28; 

Uc  =  0.0; 

Vc  =  0.0; 

%  Kinematics 

New_X(ii+l)  =  New_X(ii)  +  (Uc  +  (U/3.28)*cos(new_psi(ii))  -  new_v(ii)... 
/3.28*sin(new_psi(ii))  )*dt; 

New_Y(ii+l)  =  New_Y(ii)  +  (Vc  +  (U/3.28)*sin(new_psi(ii))  +  new_v(ii)... 
/3.28*cos(new_psi(ii))  )*dt; 

%  Check  to  See  if  we  are  Within  the  Watch  Radius  (set  to  1  foot  here) 

if(sqrt(N  ew_X_W  ay_Error(ii)^2 . 0  +  N  ew_Y_W  ay_Error(ii)^2 .0) . . . 

<=  1  I  new_s(ii)  <  0.0), 

%  Next  Line  ends  mission  if  within  Watch  Radius. 

disp(sprintf('WayPoint  %d  Reached',))));  break; 
i  fO) otracks) , 

PLOTPART  =  2; 
disp(sprintf('PLOT_PART  =  2')); 
break; 
end; 

New_PrevX_Way_cO)+l)  =  New_X_Way_cO)); 
New_PrevY_Way_cO)+l)  =  New_Y_Way_cO)); 
end; 
end 
end 

dr(i+l)  =  dr(i); 
cte(i+l)  =  cte(i); 
s(i+l)  =  s(i); 
ss(i+l)  =  ss(i); 

%new_dr(ii+l)  =  new_dr(ii); 

%new_cte(ii+l)  =  new_cte(ii); 

%new_s(ii+l)  =  new_s(ii); 
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%new_ss(ii+l)  =  new_ss(ii); 

%  Plotting 

if  PLOTPART  —  1, 

%  Plot  of  Time  vs  Rudder  Angle  &  Vehiele  Heading 
figure(l); 

plot(t([l:i+l]),psi*  180/pi); 
hold; 

plot(t([l  :i+l]),dr*  180/pi, 'r:');grid; 

title('Time  vs  Rudder  Angle  and  Vehicle  Heading'); 

xlabel('Time  (sec)');  ylabel('Rudder  Angle/Vehicle  Heading  (degrees)'); 

legendC Vehicle  Heading',  'Rudder  Angle'); 

print  -tiff  -depsc  figure  lb 

hold;zoom  on; 

%  Plot  of  Time  vs  CTE,  Distance  to  Go  to  Projected  Track,  Radial 
%  Distance  to  Next  Waypoint 

figure(2); 

plot(t([l:Hl]),cte); 

hold; 

plot(t([l:Hl]),s,'r:'); 

plot(t([l:i-G]),ss,'g-');grid; 

title('Time  vs  Cross  Track  Error') 

xlabel('Time  (sec)');ylabel('Distance  (feet)'); 

legend('Cross  Track  Error',  'Distance  to  Go  Projected  to  Track', ... 

'Radial  Distance  to  Go  to  Next  Way  Point'); 
print  -tiff  -depsc  figure2b 
hold;zoom  on; 

elseif  PLOTPART  —  0, 

%  Plot  of  Time  vs  Rudder  Angle  &  Vehicle  Heading  for  Rendezvous  Mission 
figure(3); 

plot(t([  1 :  i-i- 1  ]),psi*  1 80/pi,  'y'); 
hold; 

plot(tt([l:ii-i-l]),new_psi*  180/pi,  'y:'); 

plot(t([  1 :  i-l- 1  ]),dr*  1 80/pi,'r'); 

plot(tt([  1  :ii]),new_dr*  1 80/pi,  'r: '); 

title('Time  vs  Rudder  Angle  and  Vehicle  Heading  -  O'); 

xlabel('Time  (sec)');  ylabel('Rudder  Angle/Vehicle  Heading  (degrees)'); 

legendC  Vehicle  Heading  Before  Mission  Abort',... 

'Vehicle  Heading  After  Mission  Abort',... 

'Rudder  Angle  Before  Mission  Abort',... 

'Rudder  Angle  After  Mission  Abort'); 
print  -tiff  -depsc  figureSb 
hold;grid; 

%  Plot  of  Time  vs  CTE,  Distance  to  Go  to  Projected  Track,  Radial 
%  Distance  to  Next  Waypoint  for  Rendezvous  Mission 

figure(4); 
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plot(t([l:i+l]),  cte,  'y'); 
hold; 

plot(tt([l:ii]),  new  cte,  'y:'); 
plot(t([l:i+l]),  s,'r'); 
plot(tt([l:ii]),  new  s,  'r:'); 
plot(t([l:i+l]),  ss,'g');grid; 
plot(tt([l:ii]),  new  ss,  'g:'); 
title('Time  vs  Cross  Track  Error  -  O') 
xlabel('Time  (sec)');  ylabel('Distance  (feet)'); 
legend('Cross  Track  Error  Before  Abort',... 

'Cross  Track  Error  After  Abort',... 

'Distance  to  Go  Projected  to  Track  Before  Abort', ... 

'Distance  to  Go  Projected  to  Track  After  Abort',... 

'Radial  Distance  to  Go  to  Next  Way  Point  Before  Abort',... 

'Radial  Distance  to  Go  to  Next  Way  Point  After  Abort'); 
print  -tiff  -depsc  figuredb 
hold;zoom  on; 

end; 

%  Plot  of  Actual  Track  and  Planned  Track 

%  Modified  on  05  Feb  2002  -  To  include  waypoint2.m  modifications. 

if  PLOTPART  —  1, 
figure(5); 

plot(Y,X,'b-');grid;  %  Actual  Track 

title('ARlES  Track  -  Actual  and  Planned'); 
xlabel('Y  (feet)');ylabel('X  (feet)'); 
hold; 

%  Planned  Track 

plot([ Y_Way_c(  1 )  PrevY_Way_c(  1  )],[X_Way_c(  1 )  PrevX_Way_c(  1  )],'r'); 
for  ii=2:No_tracks, 

plot([Y_Way_c(ii)  Y_Way_c(ii-l)],[X_Way_c(ii)  X_Way_c(ii-l)],'r'); 
end; 

legend('Actual  Track',  'Planned  Track',4); 
print  -tiff  -depsc  figure5b 
hold;  zoom  on 

elseif  PLOT  PART  ==  0  |  PLOT  PART  —  2, 

%  Plot  of  Planned  Track,  Track  after  Mission  Change,  Rendezvous 
%  Point  and  Initial  Track 

figure(6); 

plot(Y,X,'b-');grid;  %  Actual  Track 

title('ARlES  Track  -  Actual  and  Planned'); 
xlabel('Y  (feet)');ylabel('X  (feet)'); 
hold; 

plot(New_Y,  New_X,'g-.'); 
plot(New_Y_Way_c,  New_X_Way_c,'d'); 

%  Planned  Track 

plot([Y_Way_c(l)PrevY_Way_c(l)],[X_Way_c(l)PrevX_Way_c(l)],'r'); 
for  ii=2:No_tracks, 

plot([ Y_W ay_c(ii)  Y_Way_c(ii- 1 )] ,  [X_Way_c(ii)  X_W ay_c(ii- 1 )] ,  'r') ; 
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end; 

legend('Initial  Track',  'Track  After  Mission  Change'... 

,  'Rendezvous  Point',  'Planned  Track',4); 

AXIS([-100  250  -80  60]) 
print  -tiff  -depsc  figureOb 
hold;  zoom  on 

%  plot([Y_Way_c(2)  Y_Way_c(l)],[X_Way_c(2)  X_Way_c(l)],'r'); 

%  plot([Y_Way_c(3)  Y_Way_c(2)],[X_Way_c(3)  X_Way_c(2)],'r'); 

%  plot([Y_Way_c(4)  Y_Way_c(3)],[X_Way_c(4)  X_Way_c(3)],'r'); 

%  plot([Y_Way_c(5)  Y_Way_c(4)],[X_Way_c(5)  X_Way_c(4)],'r'); 

end 
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APPENDIX  C.  MATLAB  FILE  COEFFICIENTS.M 


This  appendix  contains  a  MATLAB  file  that  finds  the  coefficients  of  the 
longitudinal  equation  of  motion  using  the  method  of  least  squares.  It  then  disregards 
those  values  and  uses  the  hand-manipulated  values  in  order  to  produce  a  model  for  speed. 
%  This  version  doubles  all  the  RPS  values  based  on  the  Teenadyne  Data. 

%  Loads  the  3  .d  fdes  from  the  experiment  eondueted  on  17APR02. 

load  d041702_03.d; 
load  d041702_04.d; 
load  d041702_05.d; 
a  =  d041702_03;  %  L‘mn 

b  =  d041702_04;  %2‘’‘^mn 

e  =  d041702_05;  %3^‘‘mn 

%  a(:,3 1)  is  left  serew  voltage  for  L‘  run 
%  a(:,32)  is  right  serew  voltage  for  L‘  run 
%  a(:,17)  is  u  in  m/s 

%  First  Caleulate  Average  Thruster  RPS 

a(:,39)  =  ((a(:,31)*133.8047)+(a(:,32)*124.4615))/60;  %Avg  rps,  Run  1 
b(:,39)  =  ((b(:,31)*133.8047)+(b(:,32)*124.4615))/60;  %Avg  rps,  Run  2 
e(:,39)  =  ((e(:,31)*133.8047)+(c(:,32)*124.4615))/60;  %Avg  rps,  Run  3 

[j,k]  =  size(a);  %  Figures  size  of  data  matriees 

[jj,  kk]  =  size(b); 

[jjj,  kkkl  =  size(e); 

ul  =  [];  u2  =  [];  u3  =[];  uul  =[];  uu2  =  [];  uu3  =[]; 
yi  =  11;  y2  =  11;  y3  =  11; 


%, 


%  EOM  for  Longitudinal  Motion: 

%  (m-X_udot_r)udot_r  =  [X  u  *  u_r  *  abs(u_r)]  +  [alpha  *  n  *  abs(n)]  - 
%  [gamma  *  abs(n)  *  u_r] 

%  (m-X_udot_r)  =  215.47  kg  =»  z  =  l/(m-X_udot_r)  =  0.004641 
%  udot  r  =  (u_r(t+l)  -  u_r(t))/dt  therefore, 

%  u_r(t+l)  -  u_r(t)  =  (z*dt)[(X_u  *  f(u_r))  +  (alpha  *  n  *  abs(n))  - 
%  (gamma  *  n  *  abs(u_r)) 

%dt  =  0.125  — »  (z*dt)  =  0.004641  *0.125. 

%  In  matrix  form:  y  =  FI  *  Theta 

%  where  FI  =  (z*dt)[f(u_r)  n*abs(n)  n*abs(u_r)]  and  Theta  =  [X  alpha  gamma]' 
%  and  y  =  u_r(t+l)  -  u_r(t) 
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z  =  0.004641; 
dt  =  0.125; 

%  Calculations  for  Run  1 


for  nl  =  2:j; 

ul(nl-l)  =  a(nl,17);  %u_r(t+l) 

nl  =nl+l; 
end 

ul  =  ul';  %  Makes  ul  a  j-1  x  1  matrix 

H11  =  [];H12  =  [];H13  =  [];H1  =  []; 


for  nnl  =  l:j-l; 

Hll(nnl)  =  (z*dt)  *  (a(nnl,17)*abs(a(nnl,17))); 
H12(nnl)  =  (z*dt)  *  (a(nnl,39)*  abs(a(nnl,39))); 
H13(nnl)  =  (z*dt)  *  (a(nnl,17)*abs(a(nnl,39))); 
uu  1  (nn  1 )  =  a(nn  1,17);  %  u_r(t) 

nnl  =  nnl+1; 
end 


uul  =  uul'; 
yl  =  ul-uul; 

HI  =  [H11'H12'H13']; 


%  Makes  uul  aj-1  x  1  matrix 
%  y  =  u_r(t+l)  -  u_r(t) 

%  Makes  H  a  j-1  x  3  matrix 


theta  hatl  =  inv(Hr  *  HI)  *  HI'  *  yl;  %  3  x  1  matrix 
error  1  =yl  -  (HI  *  theta  hatl); 

% . 


%  Calculations  for  Run  2 


for  n2  =  2:jj; 

u2(n2-l)  =  b(n2,17);  %u_r(t-l-l) 

n2  =  n2-l-l; 
end 

u2  =  u2';  %  Makes  u2  a  jj-1  x  1  matrix 

H21  =  [];H22  =  [];H23  =  [];H2  =  []; 


fornn2=  l:jj-l; 

H21(nn2)  =  (z*dt)  *  (b(nn2,17)*abs(b(nn2,17))); 
H22(nn2)  =  (z*dt)  *  (b(nn2,39)*abs(b(nn2,39))); 
H23(nn2)  =  (z*dt)  *  (b(nn2,17)*abs(b(nn2,39))); 
uu2(nn2)  =  b(nn2, 1 7);  %  u_r(t) 

nn2  =  nn2-l-l; 
end 


uu2  =  uu2'; 
y2  =  u2-uu2; 

H2  =  [H21'H22'  H23']; 


%  Makes  uu2  a  jj-1  x  1  matrix 
%  y  =  u_r(t-H)  -  u_r(t) 

%  Makes  H  a  jj-1  x  3  matrix 


theta_hat2  =  inv(H2'  *  H2)  *  H2'  *  y2;  %  3  x  1  matrix 

error2  =  y2  -  (H2  *  theta_hat2); 

% . 
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%  Calculations  for  Run  3 


for  n3  = 

u3  (n3  - 1 )  =  c(n3 , 1 7) ;  %  u_r(t+ 1 ) 

n3  =  n3+l; 
end 

u3  =  u3';  %  Makes  u3  a  jjj-1  x  1  matrix 

H31  =  [];H32  =  [];H33  =  [];H3  =  []; 


fornn3  =  l:jjj-l; 

H31(nn3)  =  (z*dt)  *  (c(nn3,17)*abs(c(nn3,17))); 
H32(nn3)  =  (z*dt)  *  (c(nn3,39)*abs(c(nn3,39))); 
H33(nn3)  =  (z*dt)  *  (c(nn3,17)*abs(c(nn3,39))); 
uu3  (nn3 )  =  c(nn3 ,17);  %  u_r(t) 

nn3  =  nn3+l; 
end 


uu3  =  uu3'; 
y3  =  u3-uu3; 

H3  =  [H31'H32'  H33']; 


%  Makes  uu3  a  jjj-1  x  1  matrix 
%  y  =  u_r(t-M)  -  u_r(t) 

%  Makes  H  a  jjj-1  x  3  matrix 


theta_hat3  =  inv(H3'  *  H3)  *  H3'  *  y3;  %  3  x  1  matrix 
error3  =  y3  -  (H3  *  theta_hat3); 


%, 


%  Summary  of  coefficients  for  all  3  rans  and  average  value. 

avg  =[]; 
format 

avg_X  =  (theta_hat  1(1,1)  -I-  theta_hat2(l,l)  -I-  theta_hat3(l,l))/3; 
avg_a  =  (theta_hat  1(2,1)  -I-  theta_hat2(2,l)  -I-  theta_hat3(2,l))/3; 
avg_g  =  (theta_hat  1(3,1)  +  theta_hat2(3,l)  +  theta_hat3(3,l))/3; 
avgs  =  [avg_X  avg_a  avg_g]; 
avgs  =  avgs'; 

all  theta  hats  =  [theta  hatl  theta_hat2  theta_hat3  avgs] 

%  all  theta  hats  = 

%  Run  1  Run  2  Run  3  Avg 

%  -13.4933  -16.7915  -16.7587  -15.6812  X_u 
%  0.4445  0.3115  0.1957  0.0793  alpha 

%  1.6506  2.6863  3.1466  1.2473  gamma 

%a(39)  is  data  for  revs 
%a(17)  is  the  speed 
%Prediction 

%  Manipulated  Values 

avg_X  =  -10.5;  %  Based  on  C  D  of  0.2 

avg_g  =  0.05; 

avg_a=0.155; 

model  ul  =  [];  model_u2  =  [];  model_u3  =  []; 
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%  Run  1  Model 
model_ul(l)  =  a(l,17); 
for  m  = 

model_ul(m+l)  =  (z*dt)*((avg_X  *  model_ul(m)  *  abs(model_ul(m)))  +... 
(avg_a  *  a(m,39)  *  abs(a(m,39)))  -  (avg_g  *  a(m,39)  *... 
abs(model_ul(m))))  +  model_ul(m); 
m  =  m  +1; 
end 

model  ul  ^model  ul'; 

%  Run  2  Model 
model_u2(l)  =  b(l,17); 
for  mm  = 

model_u2(mm+l)  =  (z*dt)*((avg_X  *  model_u2(mm)  *  abs(model_u2(mm)))+... 
(avg_a  *  b(mm,39)  *  abs(b(mm,39)))  -  (avg_g  *  b(mm,39)  *  ... 
abs(model_u2(mm))))  +  model_u2(mm); 
mm  =  mm  +1; 
end 

model_u2  =  model_u2'; 

%  Run  3  Model 
model_u3(l)  =  c(l,17); 
formmm=  l:jjj-l; 

model_u3(mmm+l)  =  (z*dt)*((avg_X  *  model_u3(mmm)  *  ... 

abs(model_u3(mmm)))  +  (avg  a  *  c(mmm,39)  *  abs(c(mmm,39)))  -... 

(avg  g  *  c(mmm,39)  *  abs(model_u3(mmm))))  +  model_u3(mmm); 
mmm  =  mmm  +1 ; 
end 

model_u3  =  model_u3'; 

figure(l) 
orient  tall 
subplot(3,l,l) 
plot(model_u  1  ( : ,  1 )) 
hold 

plot(a(2:j,17),  'r');  grid 
legend('Model',  'Actual',  4) 

titleC Vehicle  Speed  -  Run  1');  xlabel('Time  Units'); 

ylabel('Long.  Speed') 

hold 

subplot(3,l,2) 

plot(model_u2(:,l)) 

hold 

plot(b(2:jj,17),  'r');  grid 
legend('Model',  'Actual',  4) 

titleC Vehicle  Speed  -  Run  2');  xlabel('Time  Units'); 

ylabel('Long.  Speed') 

hold 

subplot(3,l,3) 
plot(model_u3  ( : ,  1 )) 
hold 

plot(c(2:jjj,17),  'r');  grid 
legend('Model',  'Actual',  4) 
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title('Vehicle  Speed  -  Run  3');  xlabel('Time  Units'); 
ylabel('Long.  Speed') 
print  -tiff  -depsc  modelplot 
hold 
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APPENDIX  D.  MATLAB  FILE  FINALRENDEZVOUS.M 

This  appendix  contains  the  MATLAB  code  that  produces  a  simulation  of  a 
rendezvous  in  space  and  time  using  a  sliding  mode  control  speed  controller. 

%  This  is  the  final  program.  It  adds  an  updated  speed  controller  based  on 
%  the  control  law  and  the  longitudinal  equation  of  motion. 

%  15  May  2002 

whitebg('k'); 

%  State  =  [v  r  psi] 
clear  all 
TRUE  =  1; 

FALSE  =  0; 

%  Converts  Degrees  to  Radians  &  Radians  to  Degrees 

DegRad  =  pi/180; 

RadDeg  =  180/pi; 

%  State  Model  Parameters 


W  =600.0; 

U=  1.4*3.28; 
g  =  32.174; 

Boy  =  500.0; 
xg  =0.125/12.0; 
m  =  W/g; 
rho  =  1.9903; 

L=  10; 


%  Weight  in  LB 

%  Forward  Speed  in  ft/s  (1.4  m/s) 

%  Gravity  in  ft/sec^2 
%  Bouyancy  ? 

%  ?? 

%  Mass 

%  Density  of  Seawater  in  slugs/ft^3 
%  Length  in  ft  of  ARIES 
Iz  =  (l/12)*m*(1.33^2  +  10^2);  %  Approx.  Using  I  =  l/12*m*(a*2  +  b^2) 
%  where  a  is  width  &  b  is  length 
Iz  =  Iz*5.0; 


%  Coefficients 


Yv  dot  =  -0.03430*(rho/2)*L^3;  %  Added  Mass  in  Sway  Coefficient. 

Yr  dot  =  -0.00178*(rho/2)*L^4;  %  Added  Mass  in  Yaw  Coefficient. 

Yv  =  -0. 10700*(rho/2)*L^2;  %  Coeff  of  Sway  Force  induced  by  Side  Slip 

Yr  =  0.01 187*(rho/2)*L^3;  %  Coeff  of  Sway  Force  induced  by  Yaw 

Ydrs  =  (0.01241*(rho/2)*L^2)/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 
Ydrb  =  (0.01241*(rho/2)*L^2)/2.0;  %  So  don't  use  these  equations 

Nv  dot  =  -0.00178*(rho/2)*L^4;  %  Added  Mass  Moment  of  Inertia  in  Sway  Coeff 
%Nr_dot  =  -0.00047*(rho/2)*L^5; 

Nr  dot  =  -Iz;  %  Added  Mass  Moment  of  Inertia  in  Yaw  Coeff 

Nv  =  -0.00769*(rho/2)*L^3;  %  Coeff  of  Sway  Moment  from  Side  Slip 

Nr  =  -0.00390*(rho/2)*LM;  %  Coeff  of  Sway  Moment  from  Yaw 

%Ndrs  =  -2.6496/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 

%Ndrb  =  1.989/2.0; 

%  Below  Modified  on  7/12/00  The  3.5  and  3.4167  is  the  Moment  Arm  Length 
%  in  Feet  -  Since  Bow  &  Stem  Lower  Rudders  Removed 
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Ndrs  =  -0.01241*(rho/2)*(L^2)*(3.5)/2.0; 

Ndrb  =  0.01241*(rho/2)*(L^2)*(3.4167)/2.0; 

%  Combining  Stem  &  Bow  Rudder  Effectivness 

Ndr  =  Ndrs  -  Ndrb; 

Ydr  =  Ydrs  -  Ydrb;  %  Cancel  Out 

%  Matrices 

ml  =  m  -  Yv  dot; 
m2  =  m*xg  -  Yr  dot; 
m3  =  m*xg  -  Nv  dot; 
m4  =  Iz  -  Nr  dot; 

Y1  =Yv; 

Y2  =  Yr; 

Y3  =  U^2*Ydr; 

N1  =Nv; 

N2  =  Nr; 

N3  =  U^2*Ndr; 

A  =  [Y1*U  Y2*U;N1*U  N2*U]; 

B  =  [Y3  N3]'; 

M  =  [ml  m2;m3  m4]; 

A1  =  inv(M)*A; 

B1  =inv(M)*B; 

AO  =  [Al(l,l)  Al(l,2)0; 

A1(2,1)A1(2,2)  0; 

0  10]; 

BO  =  [B1;0]; 
dt  =  0.125; 
t  =  [0:dt:1000]'; 
size(t); 

%  Set  initial  conditions 

start=10; 
v(l)  =0.0; 
r(l)  =0.0; 

U(l)  =1.4*3.28; 
rRM(l)=r(l); 
psi(l)  =  50.0*DegRad; 

X(l)  =  -80.0; 

Y(1)  =  0.0; 
ucom=[]; 

%  Convert  to  Feet  ? 

%  This  data  from  track.out  file  for  ARIES  in  Waiting  Pattern 
%  (12  Mar  02) 

No_tracks=4;  %  Sets  #  of  Tracks  =  #  of  Rows 

Track=[50.0  0.0  2.75  2.75  0  1.25  1.00  0  25.00  8.00  40.00 
50.0-60.0  2.75  2.75  0  1.25  1.00  0  25.00  8.00  200.00 
-70.0-60.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  200.00 
-70.0  0.0  2.75  2.75  0  1.25  1.00  0  25.00  2.00  40.00]; 


%  Initial  Side  Slip  Velocity 
%  Initial  Yaw 

%  Initial  Forward  Speed 

%  Initial  Heading  of  ARIES 
%  Initial  Position  in  meters 
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track=Track(:,l  :2);  %  Defines  track  as  Track(X,Y) 

SurfaceTime  =  Track(:,9);  %  Col  9  of  Track  is  Surface  Time  for  Pop-up 

SurfPhase  =Track(:,8);  %  Col  8  of  Track  designates  if  Pop-up 

%  This  is  the  REMUS  Search  Pattern 
%  12  Mar  02 

Y  REMUS  =  [200  50  50  200  200  50  50  200  200  50  50  200  200  50]; 

X  REMUS  =  [50  50  30  30  10  10  -10  -10  -30  -30  -50  -50  -70  -70]; 

%  Read  in  way  points  from  track  data  assumes  track  is  loaded 

for  j=l  :No_tracks, 

X_W  ay_c(j )  =  track(j ,  1 ) ; 

Y_W  ay_c(j )  =  track(j  ,2) ; 

end; 

%  Set  start  position 

PrevX_Way_c(l)  =  -80.0;  %  meters 

PrevY_Way_c(l)  =  00.0;  %  meters 

room  =  0.0; 

W_R  =10.0;  %  Sets  initial  Watch  Radius  (meters) 

a  =  -.3; 

b  =  (9/24)*a; 

x(:,l)  =  [v(l);r(l);psi(l)]; 

%  Below  are  in  British  Units  for  CTE  Sliding  Mode 
%Laml  =  0.75; 

%Lam2  =  0.5; 

Laml  =  2.0; 

Lam2  =  1.0; 

EtaFlightHeading  =1.0; 

PhiFlightHeading  =  0.5; 

%  Below  for  tanh 

Eta_CTE  =  0.1; 

EtaCTEMin  =  1.0; 

PhiCTE  =  0.5; 

Uc  =  []; 

Vc  =  []; 

PLOT  PART  =  0;  disp(sprintf('PLOT_PART  =  0')); 

%  Total  Track  Length  between  initial  waypoint  and  waypoint  (1)  -  Eq  (10) 

SegLen(l)  =  sqrt((X_Way_c(l)-PrevX_Way_c(l))^2-l-(Y_Way_c(l)... 
-PrevY_Way_c(l))^2); 

%  Track  Angle  of  first  track  -  Eq  (1 1) 

psi_track(  1 )  =  atan2(Y_Way_c(  1  )-PrevY_Way_c(  1  ),X_Way_c(  1  )-PrevX_Way_c(  1 )); 
%  Computes  track  lengths  and  track  angles  for  each  track 
for  j =2 :  N  otracks , 
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SegLen(j)  =  sqrt((X_Way_c(j)-X_Way_c(j-l))*2+(Y_Way_c(j)-... 
Y_Way_cG-l))''2); 

psi_trackG)  =  atan2(Y_Way_cG)-Y_Way_cG-l),X_Way_cG)-X_Way_cG-l)); 
end; 

Sigma  =  []; 

Depthcom  =  []; 
dr=[]; 
drl  =  []; 
drl(l)  =  0.0; 

Depth_com(l)  =  5.0; 

WayPointVertDist  com  =  [5.0  5.0  5.0  5.0  5.0]; 

SURFACETIMERACTIVE  =  FALSE; 

%  Starts  a  loop  that  computes  values  for  each  data  point  corresponding  to 
%  a  time  value  (in  this  case,  every  0.125  seconds  from  1  to  1000  seconds) 

for  i=l:length(t)-l, 

Depth_com(i)  =  WayPointVertDistcomG); 

%  Difference  between  current  vehicle  position  &  the  next 
%  waypoint  Eq(13) 

X_Way_Error(i)  =  X  Way  cG)  -  X(i); 

Y_Way_Error(i)  =  Y_Way_cG)  -  Y(i); 

%  DeWrap  psi  to  within  +/-  2.0*pi;  Makes  Pleading  Angle  to  lie  between 
%  0-360  degrees 

psi_cont(i)  =  psi(i); 
while(abs(psi_cont(i))  >  2.0*pi) 

psi_cont(i)  =  psi_cont(i)  -  sign(psi_cont(i))*2.0*pi; 
end; 

%  Cross  Track  Pleading  Error  Eq(12) 
psi_errorCTE(i)  =  psi_cont(i)  -  psi  trackG); 

%  DeWrap  psi  error  to  within  +!-  pi;  Normalized  to  Lie  between  +!-  180 
%  degrees 

while(abs(psi_errorCTE(i))  >  pi) 

psi_errorCTE(i)  =  psi_errorCTE(i)  -  sign(psi_errorCTE(i))*2.0*pi; 
end; 

%  **  Always  Calculate  this  (What  is  This?) 

Beta  =  v(i)/U(i); 

%  Beta  =  0.0; 

cpsie  =  cos(psi_errorCTE(i)-l-Beta); 
spsie  =  sin(psi_errorCTE(i)-l-Beta); 

%  Distance  to  the  ith  way  point  projected  to  the  track  line  S(t)i  - 
%  Eq  (14) 

s(i)  =  [X_Way_Error(i),Y_Way_Error(i)]*[(X_Way_cG)-... 
PrevX_Way_cGb,(Y_Way_cG)-PrevY_Way_cG))]'; 
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%  s  is  distance  to  go  projected  to  track  line 
%  (goes  from  0-100%L)  -  Eq  (14) 

s(i)  =  s(i)/SegLen(j); 

Ratio=(1.0-s(i)/SegLen(j))*  100.0;  %  Ranges  from  0-100%  of  SegLen 

%  Radial  distance  to  go  to  next  WP 

ss(i)  =  sqrt(X_Way_Error(i)^2  -I-  Y_Way_Error(i)^2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line  -  Eq  (16) 

dp(i)  =  atan2(  (Y_Way_c(j)-PrevY_Way_c(j)),(X_Way_c(j)-... 

PrevX_Way_c(j)) )-  atan2(  Y_Way_Error(i),X_Way_Error(i) ); 
if(dp(i)>pi), 

dp(i)  =  dp(i)  -  2.0*pi; 
end; 

%  Cross  Track  Error  Definition  -  Eq  (15) 
cte(i)  =  s(i)*sin(dp(i)); 

%  If  the  magnitude  of  the  CTE  Heading  exceeds  40  degrees,  a  LOS 
%  Controller  is  used. 

if(  abs(psi_errorCTE(i))  >=  40.0*pi/180.0  |  s(i)  <  0.0  ), 

LOS(i)=  1; 

psi  comLOS  =  atan2(Y_Way_Error(i),X_Way_Error(i));  %  Eq  (22) 
psi_errorLOS(i)  =  psi  comLOS  -  psi_cont(i);  %  Eq  (23) 

%  LOS  Error 

if(abs(psi_errorLOS(i))  >  pi), 

psi_errorLOS(i)  =  psi_errorLOS(i)  -  2.0*pi*psi_errorLOS(i)... 

/abs  (p  sierrorLO  S  (i)) ; 

end; 

%  Eq  (8) 

Sigma  FlightHeading  =  0.9499*(r_com  -  r(i))  -I-  0.1701*... 
psi_errorLOS(i); 

%  Eq  (9) 

dr(i)  =  -1.5435*(  2.5394*r(i)-l-  Eta_FhghtHeading*tanh... 
(SigmaFlightHeading/PhiFlightHeading)); 


else 

%  Use  CTE  Controller  if  CTE  Heading  is  less  than  40  degrees 
LOS(i)  =  0; 

if(cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 

%  Sliding  Surface  -  Eq  (20) 
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Sigma(i)  =  U(i)*rRM(i)*cpsi_e  +  Laml*U(i)*spsi_e  +  3.28*Lam2... 
*cte(i); 

%  Rudder  Input  -  Eq  (21) 

dr(i)  =  (1.0/(U(i)*b*cpsi_e))*(-U(i)*a*rRM(i)*cpsi_e  +  U(i)*... 
rRM(i)^2*spsi_e  -  Laml*U(i)*rRM(i)*cpsi_e  -  Lam2*U(i)*... 
spsi_e  -  Eta_CTE*(Sigma(i)/Phi_CTE)); 

else 

dr(i)  =  dr(i-l); 
end; 

end;  %  End  of  CTE  Controller 

%  Use  LOS  if  near  to  loiter  point 
%  if  (loiter==l)&  s(i)<10;  dr(i)=drlos(i);end; 

%  Surface  Phase  Logic  (Independent  of  LOS  or  CTE) 

if(SurfPhaseG)  ==  TRUE) 
if(SURFACE_TIMER_ACTIVE  ==  FALSE) 
if(Ratio  >  40.0) 

%  Start  a  Timer 

SURFACETIMERACTIVE  =  TRUE; 

Depth_com(i)  =  0.0; 

SurfaceWait  =  SurfaceTimeG)  +  t(i); 

Surface  Wait 
end; 
end; 
end; 

if(SURFACE_TIMER_ACTIVE  ==  TRUE) 
if(t(i)  >=  SurfaceWait) 

SURFACETIMERACTIVE  =  FALSE; 

Depth_com(i)  =  WayPointVertDistcomG); 

SurfPhaseG)  =  0; 
else 

Depth_com(i)  =  0.0; 
end; 
end; 

if(abs(dr(i))  >  0.4) 
dr(i)  =  0.4*sign(dr(i)); 
end; 

%  Model  drl  is  the  actual  lagged  rudder,  dr  is  the  rudder  command. 

%  taudr  =  0.255; 

%  drl(i+l)  =  drl(i)  +  dt*(dr(i)-drl(i))/taudr; 

%  if(abs(drl(i))  >  0.4) 

%  drl(i)  =  0.4*sign(drl(i)); 

%  end; 

%  Jay  Johnson  Model 

Yv  =-68.16; 

Yr  =406.3; 

Ydr  =  70.0; 
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Nv  =-10.89; 

Nr  =  -88.34; 

Ndr  = -35.47; 

MY  =  456.76; 

IN  =  215; 

M  =  diag([MY,IN,l]); 

AA  =  [Yv,Yr,0;Nv,Nr,0;0,l,0]; 

BB  =  [Ydr;Ndr;0]; 

A  =  inv(M)*AA; 

B  =inv(M)*BB; 

%  x_dot(:,i+l)  =  [  A(l,l)*v(i)  -l-  A(l,2)*r(i)  -I-  B(l)*drl(i); 

%  A(2,l)*v(i)  A(2,2)*r(i)  B(2)*drl(i); 

%  r(i)]; 

x_dot(:,i+l)  =  [  A(l,l)*v(i)  -l-  A(l,2)*r(i)  -I-  B(l)*dr(i); 

A(2,l)*v(i)  A(2,2)*r(i)  B(2)*dr(i); 

r(i)]; 

x(:,i+l)  =  x(:,i)+dt*x_dot(:,i); 
v(i-H)  =x(l,Nl)/12; 
r(i-H)  =x(2,i-H); 

U(i-i-l)  =  1.4*3.28;  %  Constant  speed  of  2.72  knots 

psi(i-H)  =  x(3,i-H); 
rRM(i-H)  =  r(i-M); 

%  Added 

%  rRM(i-l-l)  =  rRM(i)  -I-  dt*(a*rRM(i)  -I-  b*dr(i)); 

%  psi(i-l-l)  =  psi(i)  -I-  dt*rRM(i); 

%  Throw  in  some  Waves 
%  Ue(i)  =  -0.5*sin(2*pi*t(i)/5); 

%  Ve(i)  =  0.5*sin(2*pi*t(i)/5); 

%  Model  using  system  ID  results  from  Bay  tests 

%  rRM(i-l-l)  =  rRM(i)  -I-  dt*(a*rRM(i)  -I-  b*drl(i)); 

%  psi(i-l-l)  =  psi(i)  -I-  dt*rRM(i); 

%  side  slip  added  proprtional  to  turn  rate  from  AZORES  data  V  in  ft/see 
%  v(i-M)  =  1.0*rRM(i-tl)*3.28; 

Ue  =  0.0; 

Ve  =  0.0; 

%  Kinematies 

X(i-l-l)  =  X(i)  -I-  (Ue  -I-  (U(i)/3.28)*eos(psi(i))  -  v(i)/3.28*sin(psi(i))... 
)*dt; 

Y(i-l-l)  =  Y(i)  -I-  (Ve  -I-  (U(i)/3.28)*sin(psi(i))  -I-  v(i)/3.28*eos(psi(i))... 
)*dt; 

%  This  should  abort  @  30  seeonds  if  input  is  empty  or  'Y' 

%  This  modifieation  done  on  05  Feb  2002  -  original  file  is  waypointl.m 
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ifi  —  240, 

K  =  input(... 

'Is  Rendezvous  Required?  (Enter  1  for  Yes,  0  for  No)— »'); 
if  isempty(K)==l;  K  =  1;  break;  end; 
if  K  ==  1;  break; 
else  i  =  i;  end; 
end 

%  Check  to  See  if  we  are  Within  the  Watch  Radius 

if(sqrt(X_Way_Error(i)^2.0  +  Y_Way_Error(i)^2.0)  <=  W_R  |  s(i)  <  0.0), 
disp(sprintf('WayPoint  %d  Reached',))); 
i  f(j  ==No_tracks) , 

PLOTPART  =  1; 
disp(sprintf('PLOT_PART  =  1')); 
break; 
end; 

PrevX_Way_c(j+l)  =  X_Way_c(j); 

PrevY_Way_c(j+l)  =  Y_Way_c(j); 

j=j+i; 

end; 

end;  %end  of  i  loop 

%  Requests  Rendezvous  Point  Information 

%  This  modification  done  on  05  Feb  2002  -  original  file  waypoint2.m 

ifj  ==No_tracks, 

disp(sprintf('Mission  Complete')); 
else 

newrcom  =  0.0; 
new_v(l)  =  v(i+l); 
new_r(l)  =  r(i+l); 
new_rRM(l)  =  new_r(l); 
new_psi(l)  =  psi(i+l); 

New_X(l)=X(i+l); 

New_Y(l)=Y(i+l); 

New_No_Tracks  =  1; 

New  Track  =  input('Enter  12  Column  Track,  i.e.,  [1  1  ...]—»'); 
newtrack  =  New_Track(:,  1 :2); 
newSurfaceTime  =  New_Track(:,9); 
newSurfphase  =  New_Track(:,8); 

new_U(l)  =  U(i+l)/3.28;  %  Sets  Initial  Rendezvous  Speed  to  1.4  m/s 

%  new_U  in  meters/sec. 

new_time(l)  =  New_Track(:,12);  %  Desired  Time  to  Rendezvous  in  seconds 

overall_distance_travelled(l)  =  0; 

distance_travelled(l)  =  0; 

time_used(l)  =  0; 

time_remaining(l)  =  new_time(l); 
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real_time(l)  =  0; 

accel(l)  =  0.03;  %  Sets  Acceleration  to  0.03  m/s^2 

decel(l)  =  0.0249; 
ncom(l)  =  12; 

%  Sets  Deceleration  to  0.0249  m/s''2 

for  jj  =  l:New_No_Tracks, 

New_X_Way_c(jj)  =  new_track(jj,l); 

New_Y_Way_c(jj)  =  new_track(jj,2); 
end; 

New_PrevX_Way_c(l)  =  X(i+1);  %  Sets  Abort  Posit  as  start  of  new 

New_PrevY_Way_c(l)  =  Y(i+1);  %  track. 

%  Below  for  tanh 
newEtaCTE  =  0.1; 
newEtaCTEMin  =  1.0; 
newPhiCTE  =  0.5; 

PLOT  PART  =  0;  disp(sprintf('PLOT_PART  =  0')); 
new_x(:,l)  =  [new_v(l);  new_r(l);  new_psi(l)]; 

%  Total  Track  Length  between  abort  point  and  rendezvous  point 

New_SegLen(l)  =  sqrt((New_X_Way_c(l)-New_PrevX_Way_c(l))''2+... 
(New_Y_Way_c(l)  -  New_PrevY_Way_c(l))^2) 

%  Track  Angle  of  track  between  abort  point  and  rendezvous  point 

new_psi_track(l)  =  atan2(New_Y_Way_c(l)-New_PrevY_Way_c(l),... 

N  ew_X_Way_c(  1  )-New_PrevX_Way_c(  1 )); 

%  Determines  if  the  Mission  is  Feasible  -  Can  Distance  be  covered  in 
%  time  required  traveling  at  maximum  speed  of  3.5  knots  or  Is  time 
%  required  to  be  at  the  rendezvous  too  much  for  vehicle  travelling  at 
%  minimum  speed  of  0.5  knots?  (28  Feb  02) 

%  Added  10  extra  meters  to  account  for  curvature  of  path  (14  May  02) 

ifnew_time  >  ((New_SegLen(l)+10)/(1.8))  &... 
new_time  <  ((New_SegLen(l)+10)/(0.2571)), 
disp('Mission  Feasible'); 
else  disp('Mission  Not  Feasible'); 

break;  %  Ends  Simulation  if  Not  Feasible 

end 

%  Determines  if  there  is  enough  length  to  achieve  deceleration  or 
%  acceleration  -  (12  Mar  02) 

decel_Len  =  abs(((0.2571)^2  -  (1.4)^2)7(2*0.0249)); 
accel_Len  =  ((1.8)^2  -  (1.4)^2)/(2*0.03); 

time  of  decel  =  1 14;  %Time  to  decel  from  1.4  m/s  to  0.2571  m/s 
timeleft  =  New_Track(:,12)-time_of_decel; 
distanceremaining  =  New_SegLen(l)-decel_Len; 
if  distance  remaining  >  0.2571  *  time  left, 
disp('Mission  Feasible  for  Deceleration'); 
else  disp('Mission  Not  Feasible  for  Deceleration'); 
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break; 

end 

%  Starts  loop  that  eomputes  values  for  eaeh  data  point  eorresponding  to 
%  a  time  value  along  new  traek 

newSigma  =  []; 
newDeptheom  =  []; 
new_dr=[]; 
new  drl  =  [];  n  =  []; 
new_drl(l)  =  0.0; 
new_Depth_eom(l)  =  5.0; 

new  WayPointVertDist  eom  =  [5.0  5.0  5.0  5.0  5.0]; 
new_SURFACE_TIMER_ACTIVE  =  FALSE; 
tt  =  [t(i+l):dt:3*new_time]'; 
size(tt); 

%  Start  of  Loop  for  Rendezvous  Point 
for  ii  =  l:length(tt)-l, 

new_Depth_eom(ii)  =  new_WayPointVertDist_eom(jj); 
New_X_Way_Error(ii)  =  New_X_Way_c(jj)  -  New_X(ii); 
New_Y_Way_Error(ii)  =  New_Y_Way_e(jj)  -  New_Y(ii); 
new_psi_eont(ii)  =  new_psi(ii); 
while(abs(new_psi_eont(ii))  >  2.  0*pi) 

new_psi_eont(ii)  =  new_psi_eont(ii)  -  sign(new_psi_eont(ii))*... 

2.0*pi; 

end; 

%  Cross  Traek  Pleading  Error  Eq(12) 

new_psi_errorCTE(ii)  =  new_psi_cont(ii)  -  new_psi_track(jj); 

%  DeWrap  psi  error  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 
%  180  degrees 

while(abs(new_psi_errorCTE(ii))  >  pi) 

new_psi_errorCTE(ii)  =  new_psi_errorCTE(ii)  -  sign(... 
new_psi_errorCTE(ii))  *2 . 0  *pi; 

end; 

%  **  Always  Caleulate  this  (What  is  This?) 
newBeta  =  new_v(ii)/new_U(ii); 

%  Beta  =  0.0; 

newepsie  =  eos(new_psi_errorCTE(ii)+new_Beta); 
newspsie  =  sin(new_psi_errorCTE(ii)+new_Beta); 

%  Distanee  to  the  ith  way  point  projeeted  to  the  traek  line  S(t)i 
%  -  Eq  (14) 

new_s(ii)  =  [New_X_Way_Error(ii),New_Y_Way_Error(ii)]*[(... 

N  e  w_X_  W  ay_e  G  j )  -N  ew_Pre  vX_  W  ay_eG  j )) ,  (N  e  w_  Y_  W  ay_e  G  j ) . . . 
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-New_PrevY_Way_c(jj))]'; 

%  Calculates  the  Overall  Distance  Travelled,  Distance  Travelled, 

%  Time  Used  Overall,  Time  Remaining,  Switches  velocity  from  0.5 
%  knots  to  3.5  knots  depending  on  Time  Remaining  and  Distance 
%  Remaining  while  taking  into  account  acceleration/deceleration. 

%  (28  Feb  02) 

ifii  ==  1, 

overall_distance_travelled(ii)  =  0; 
distance_travelled(ii)  =  0; 
time_used(ii)  =  0; 
time_remaining(ii)  =  new_time(ii); 
real_time(ii)  =  0; 
elseif  ii  ==  2, 

overall_distance_travelled(ii)  =  New_SegLen(ii)  -  new_s(ii-l); 
distance_travelled(ii)  =  overall_distance_travelled(ii); 
%time_used(ii)  =  distance_travelled(ii)/(new_U(ii)/3.28); 
time_remaining(ii)  =  time_remaining(ii-l)-dt;%time_used(ii); 
real_time(ii)  =  new_time(ii)  -  time_remaining(ii); 
else 

overall_distance_travelled(ii)  =  New_SegLen(ii)  -  new_s(ii-l); 
distance_travelled(ii)  =  overall_distance_travelled(ii)  -... 

overall_distance_travelled(ii- 1 ); 
time_used(ii)  =  distance_travelled(ii)/(new_U(ii)/3.28); 
time_remaining(ii)  =  time_remaining(ii-l)  -  dt;%time_used(ii); 
real_time(ii)  =  new_time(ii)  -  time_remaining(ii); 
end 

%  s  is  distance  to  go  projected  to  track  hne(goes  from  0-100%L) 

%  -  Eq  (14) 

new_s(ii)  =  new_s(ii)/New_SegLen(jj); 

if  (time_remaining(ii)<0.125),  disp('mission  out  of  time'),... 
break,  end; 

%  Determines  what  speed  to  set  the  vehicle  at 
%time_available(ii)  =  new_s(ii)/(new_U(ii)/3.28); 
ucom(ii)=new_s(ii)/ time_remaining(ii) ; 

ifucom(ii)  >  1.8  ; 

ucom(ii)  =  1.8  ;  %  Max  velocity  is  3.5  Knots 

end 

ifucom(ii)  <  0.2571  ; 

ucom(ii)  =  0.2571  ;  %  Min  velocity  is  0.5  Knots 

end 


%  New  Control  Law  added  for  Longitudinal  Equation  of  Motion  - 
%  14  May  2002 

new_sigma(ii)  =  new_U(ii)-ucom(ii); 
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new_phi(ii)  =  0.1;tau=0.1;iicommax=22; 


ncom2(ii)  =  (1.39*(accel(ii)*0-800*tanh(new_sigma(ii)/... 

new_phi(ii))+67.74*(new_U(ii)*abs(new_U(ii))))); 

ncom(ii)=sqrt(abs(ncom2(ii)))*sign(iicom2(ii)); 

%  Limits  propeller  speed  to  22  rps 

if  (abs(neom(ii))>neommax), 

neom(ii)=neommax*  sigii(neom(ii)) ; 
end; 

%  always  +ve  in  these  simulations 
%  solve  longitudinal  dynamics 

new_U(ii+l)  =  (0.004641*dt*(-10.5*(new_U(ii)*abs(new_U(ii)))+... 
0.155*(ncom(ii)*abs(ncom(ii)))  -  0.05*ncom(ii)*new_U(ii)))... 
+new_U(ii); 

New_SegLen(ii+l)  =  New_SegLen(ii); 
new_time(ii+l)  =  new_time(ii); 
accel(ii+l)  =  accel(ii); 

%  Ranges  from  0-100%  of  SegLen 
Ratio=(l  .0-new_s(ii)/New_SegLen(jj))*  100.0; 

%  Radial  distance  to  go  to  next  WP 

new_ss(ii)  =  sqrt(New_X_Way_Error(ii)''2  -I-  New_Y_Way_Error(ii)^2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line 
%  -Eq(16) 

new_dp(ii)  =  atan2(  (New_Y_Way_c(jj)-New_PrevY_Way_c(jj)),(... 
New_X_Way_c(jj)-New_PrevX_Way_c(jj)) )-  atan2... 

(N ew_Y_W ay_Error(ii),N ew_X_Way_Error(ii) ) ; 
if(new_dp(ii)  >  pi), 

new_dp(ii)  =  new_dp(ii)  -  2.0*pi; 
end; 

%  Cross  Track  Error  Definition  -  Eq  (15) 

new_cte(ii)  =  new_s(ii)*sin(new_dp(ii)); 

%  If  the  magnitude  of  the  CTE  Heading  exceeds  40  degrees,  a 
%  LOS  Controller  is  used. 

if(  abs(new_psi_errorCTE(ii))  >=  40.0*pi/180.0  |  new_s(ii)  <  0.0  ), 
new_LOS(ii)  =  1; 

new_psi_comLOS  =  atan2(New_Y_Way_Error(ii),... 

N  ew_X_W  ay_Error(ii)) ; 

new_psi_errorLOS(ii)  =  new_psi_comLOS  -  new_psi_cont(ii); 
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if(abs(new_psi_errorLOS(ii))  >  pi), 

new_psi_errorLOS(ii)  =  new_psi_errorLOS(ii)  -  2.0*pi*... 
new_psi_errorLOS(ii)/abs(new_psi_errorLOS(ii)); 

end; 

%  Eq  (8) 

new  Sigma  FlightHeading  =  0.9499*(new_r_com  -  new_r(ii))  +... 
0.1701*new_psi_errorLOS(ii); 

%  Eq  (9) 

new_dr(ii)  =  -1.5435*(  2.5394*new_r(ii)+  Eta  FlightHeading*... 
tanh(new_Sigma_FlightF[eading/Phi_FlightF[eading)); 


%  Use  CTE  Controller  if  CTE  Pleading  is  less  than  40  degrees 
new_LOS(ii)  =  0; 

if(new_cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 

%  Sliding  Surface  -  Eq  (20) 

new_Sigma(ii)  =  new_U(ii)*3.28*new_rRM(ii)*new_cpsi_e... 

+  Laml*new_U(ii)*3.28*new_spsi_e  +  3.28*Lam2*... 
new_cte(ii); 

%  Rudder  Input  -  Eq  (21) 

%  new_dr(ii)  =  (1.0/(new_U(ii)*b*new_cpsi_e))*... 

%  (-new_U(ii)*a*new_rRM(ii)*new_cpsi_e  +  new_U(ii)*... 

%  new_rRM(ii)^2*new_spsi_e  -  Laml*new_U(ii)*... 

%  new_rRM(ii)*new_cpsi_e  -  Lam2*new_U(ii)*... 

%  new  spsi  e  -new_Eta_CTE*(new_Sigma(ii)/new_Phi_CTE)); 
%  changes  because  new  U  is  now  in  m/sec. 

new_dr(ii)  =  (1.0/(new_U(ii)*3.28*b*new_cpsi_e))*... 
(-new_U(ii)*3.28*a*new_rRM(ii)*new_cpsi_e  +... 
new_U(ii)*3.28*new_rRM(ii)^2*new_spsi_e  - ... 
Laml*new_U(ii)*3.28*new_rRM(ii)*new_cpsi_e  -  ... 
Lam2*new_U(ii)*3.28*new_spsi_e  -new  Eta  CTE*... 
(new_Sigma(ii)/new_Phi_CTE)); 


else 

new_dr(ii)  =  new_dr(ii-l); 
end; 

end;  %  End  of  CTE  Controller 

%  Use  LOS  if  near  to  loiter  point 

%  if  (loiter==l)&  new_s(ii)<10;  new_dr(ii)=new_drlos(ii);end; 
%  Surface  Phase  Logic  (Independent  of  LOS  or  CTE) 
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if(SurfPhase  ==  TRUE) 

if(new_SURFACE_TIMER_ACTIVE  ==  FALSE) 
if(Ratio  >  40.0) 

%  Start  a  Timer 

new_SURFACE_TIMER_ACTIVE  =  TRUE; 
new_Depth_com(ii)  =  0.0; 
newSurfaceWait  =  new_SurfaceTime(l)  +  tt(ii); 
newSurfaceWait 
end; 
end; 
end; 

if(new_SURFACE_TIMER_ACTIVE  ==  TRUE) 
if(tt(ii)  >=  new  SurfaceWait) 

new_SURFACE_TIMER_ACTIVE  =  FALSE; 
new_Depth_com(ii)  =  new_WayPointVertDist_com(l); 
new_SurfPhase(l)  =  0; 
else 

new_Depth_com(ii)  =  0.0; 
end; 
end; 

if(abs(new_dr(ii))  >  0.4) 

new_dr(ii)  =  0.4*sign(new_dr(ii)); 
end; 

%  Model  drl  is  the  actual  lagged  rudder,  dr  is  the  rudder  command. 

%  taudr  =  0.255; 

%  drl(i+l)  =  drl(i)  +  dt*(dr(i)-drl(i))/taudr; 

%  if(abs(drl(i))  >  0.4) 

%  drl(i)  =  0.4*sign(drl(i)); 

%  end; 

new_x_dot(:,ii+l)  =  [  A(l,l)*new_v(ii)  +  A(l,2)*new_r(ii)  +  B(l)*... 
new_dr(ii);  A(2,l)*new_v(ii)  +  A(2,2)*new_r(ii)  +  B(2)*... 
new_dr(ii);  new_r(ii)]; 

new_x(:,ii+l)  =  new_x(:,ii)+dt*new_x_dot(:,ii); 
new_v(ii+l)  =  new_x(l,ii+l)/12; 
new_r(ii+l)  =  new_x(2,ii+l); 
new_psi(ii+l)  =  new_x(3,ii+l); 
new_rRM(ii+l)  =  new_r(ii+l); 

%  Added 

%  rRM(i+l)  =  rRM(i)  +  dt*(a*rRM(i)  +  b*dr(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  Throw  in  some  Waves 
%  Uc(i)  =  -0.5*sin(2*pi*t(i)/5); 

%  Vc(i)  =  0.5*sin(2*pi*t(i)/5); 

%  Model  using  system  ID  results  from  Bay  tests 

%  rRM(i+l)  =  rRM(i)  +  dt*(a*rRM(i)  +  b*drl(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  side  slip  added  proprtional  to  turn  rate  from  AZORES  data  V  in 
%  ft/sec 

%  v(i+l)  =  1.0*rRM(i+l)*3.28; 
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Uc  =  0.0; 

Vc  =  0.0; 

%  Kinematics  note  new  U  is  in  meters  /sec,  new  v  is  in  ft/sec 
%  hold  over  from  long  time  ago 

New_X(ii+l)  =  New_X(ii)  +  (Uc  +  (new_U(ii))*cos(new_psi(ii)) 
new_v(ii)/3.28*sin(new_psi(ii))  )*dt; 

New_Y(ii+l)  =  New_Y(ii)  +  (Vc  +  (new_U(ii))*sin(new_psi(ii))... 

+  new_v(ii)/3.28*cos(new_psi(ii))  )*dt; 

%  Check  to  See  if  we  are  Within  the  Watch  Radius  (set  to  1 
%  meter  here) 

if(sqrt(New_X_Way_Error(ii)*2.0  +  New_Y_Way_Error(ii)*2.0)... 
<=  1  I  new_s(ii)  <  0.0), 

%  Next  Line  ends  mission  if  within  Watch  Radius. 

disp(sprintf('Rendezvous  Point  Reached'));  break; 
if(jj==No_tracks), 

PLOTPART  =  2; 
disp(sprintf('PLOT_PART  =  2')); 
break; 
end; 

New_PrevX_Way_c(jj+l)  =  New_X_Way_c(jj); 
New_PrevY_Way_c(jj+l)  =  New_Y_Way_c(jj); 
end 

end  %end  of  ii  loop 
end  %  if  j=No_Tracks 

%  Plotting 

if  PLOT  PART  —  1, 

%  Plot  of  Time  vs  Rudder  Angle  &  Vehicle  Heading 

figure(l);  elf 
orient  tall 

plot(t([  1  :i+l]),psi*  1 80/pi); 
hold; 

plot(t([l  :i+l]),dr*l  80/pi, 'r:');grid; 

title('Time  vs  Rudder  Angle  and  Vehicle  Heading'); 

xlabel('Time  (sec)');  ylabel('Rudder  Angle/Vehicle  Heading  (degrees)'); 

legend('Vehicle  Heading',  'Rudder  Angle'); 

print  -tiff  -depsc  figurel  wpS 

hold;zoom  on; 

%  Plot  of  Time  vs  CTE,  Distance  to  Go  to  Projected  Track,  Radial 
%  Distance  to  Next  Waypoint 

figure(2);  elf 
orient  tall 
plot(t([l:i-G]),cte); 
hold; 
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plot(t([l:i+l]),s,'r:'); 

plot(t([l  :i+l]),ss,'g-');grid; 

title('Time  vs  Cross  Track  Error') 

xlabel('Time  (sec)');ylabel('Distance  (meters)'); 

legend('Cross  Track  Error',  'Distance  to  Go  Projected  to  Track', ... 

'Radial  Distance  to  Go  to  Next  Way  Point'); 
print  -tiff  -depsc  figure2_wp5 
hold;zoom  on; 

figure(3);  clf 
orient  tall 

plot(t([l:i-G]),U);  grid; 

title('Time  vs  Forward  Speed') 

xlabel('Time  (sec)');  ylabel('Forward  Speed  (m/s)'); 

elseif  PLOTPART  —  0, 

%  Plot  of  Time  vs  Rudder  Angle  &  Vehicle  Heading  for  Rendezvous  Mission 

figure(4);  clf 
orient  tall 

plot(t([l:i-i-l]),psi*  180/pi,  'g'); 
hold; 

plot(real_time([l:ii])+30,new_psi([l:ii])*  180/pi,  'g:'); 
plot(t([l:i]),dr*180/pi,'r'); 

plot(real_time([l:ii-l])+30,new_dr([l:ii-l])*  180/pi,  'r:'); 
title('Time  vs  Rudder  Angle  and  Vehicle  Heading'); 
xlabel('Time  (sec)');  ylabel('Rudder  Angle/Vehicle  Heading  (degrees)'); 
legendC Vehicle  Heading  Before  Mission  Abort',... 

'Vehicle  Heading  After  Mission  Abort',... 

'Rudder  Angle  Before  Mission  Abort',... 

'Rudder  Angle  After  Mission  Abort'); 
print  -tiff  -depsc  figure4_wp5 
hold;grid; 

%  Plot  of  Time  vs  CTE,  Distance  to  Go  to  Projected  Track,  Radial 
%  Distance  to  Next  Waypoint  for  Rendezvous  Mission 

figure(5);  clf 
orient  tall 

plot(t([l:i]),  cte,  'g'); 
hold; 

plot(real_time([l:ii-l])+30,  new_cte([l:ii-l]),  'g:'); 
plot(t([l:i]),  s,'r'); 

plot(real_time([l:ii-l])+30,  new_s([l:ii-l]),  'r:'); 
plot(t([  1  :i- 1]),  ss([  1  :i- l]),'b');grid; 
plot(real_time([l:ii-l])+30,  new_ss([l:ii-l]),  'b:'); 
title('Time  vs  Cross  Track  Error') 
xlabel('Time  (sec)');  ylabel('Distance  (meters)'); 
legend('Cross  Track  Error  Before  Abort',... 

'Cross  Track  Error  After  Abort',... 

'Distance  to  Go  Projected  to  Track  Before  Abort', ... 

'Distance  to  Go  Projected  to  Track  After  Abort',... 

'Radial  Distance  to  Go  to  Next  Way  Point  Before  Abort',... 
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'Radial  Distance  to  Go  to  Next  Way  Point  After  Abort'); 
print  -tiff  -depsc  figure5_wp5 
hold;zoom  on; 

figure(6);  elf 
orient  tall 

%  subplot(  1,2,1)  %  In  order  to  produce  Figure  17  in  thesis 
plot(t([l:i-G]),U/3.28,  'b*'); 
hold; 

plot(real_time([l:ii-l])+30,new_U([l:ii-l]),'r.',... 

real_time([l:ii-l])+30,ucom([l:ii-l]),'m');  grid; 

%  AXIS([0  30+new_time(l)  0  2]); 

title('Time  vs  Forward  Speed') 

xlabel('Time  (sec)');  ylabel('Forward  Speed  (m/s)'); 

legend('Original  Speed',  'Rendezvous  Speed',  'Command  Speed'); 

hold; 

%  In  order  to  produce  Figure  17  in  thesis 
%  subplot(l,2,2) 

%  plot(real_time(l:ii)+30,  ncom(l:ii),'b.');  grid 
%  title('Propeller  Speed') 

%  xlabel('Time  (sec)');  ylabel('Propeller  Speed  (rps)') 

%  axis([30  100  12  22]) 

print  -tiff  -depsc  figure6_wp5 

%  3-D  Plot 

figure(7);  elf 
orient  tall 

plot3(Y,  X,  t([l:Dl]),  'b');  grid; 
title('Time  Space  Plot') 
xlabel('Y  (meters)'); 
ylabel('X  (meters)'); 
zlabel('Time  (seconds)'); 
hold; 

plot3(New_Y([l:ii]),  New_X([l:ii]),  real_time([l:ii])+30,  'rx'); 
plot3(New_Y_Way_c,  New_X_Way_c,  new_time(ii)+30,  'gd'); 
legend('Original  Track',  'New  Track',  'Rendezvous') 
print  -tiff  -depsc  figure7_wp5 
hold; 

%  figure(8);  elf 
%  orient  tall 
%  plot(t([l:i+l]),X,'r'); 

%  hold; 

%  plot(real_time([l:ii])+30,  New_X([l:ii]),'r:'); 

%  plot(t([l:i+l]),Y,'g'); 

%  plot(real_time([l  :ii])+30,  New_Y([l  :ii]),'g:');  grid; 

%  title('Time  vs  Postilion'); 

%  xlabel('Time  (sec)');  ylabel('Position  (meters)'); 

%  legend('Original  X',  'Rendezvous  X',  'Original  Y',... 

%  'Rendezvous  Y',2); 

%  hold; 

end; 
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%  Plot  of  Actual  Track  and  Planned  Track 

%  Modified  on  05  Feb  2002  -  To  include  waypoint2.m  modifications. 

if  PLOTPART  —  1, 
figure(9);  elf 
orient  tall 

plot(Y,X,'b-');grid;  %  Actual  Track 

title('ARIES  Track  -  Actual  and  Planned'); 
xlabel('Y  (meters)');ylabel('X  (meters)'); 
hold; 

%  Planned  Track 

plot([ Y_Way_c(  1 )  PrevY_Way_c(  1  )],[X_Way_c(  1 )  PrevX_Way_c(  1  )],'r'); 
plot(Y_REMUS,  X  REMUS,  'g'); 
axis([-100  220  -120  60]); 
for  ik=2:No_tracks, 

p lot( [ Y_W ay_c(ik)  Y_ W ay  e (ik- 1 )] ,  [X_ W ay_c(ik) . . . 
X_Way_c(ik-l)],'r'); 

end; 

legend('Actual  Track  -  ARIES',  'Planned  Track  -  ARIES',... 

'REMUS  Path',4); 
print  -tiff  -depsc  figure9_wp5 
hold;  zoom  on 

elseif  PLOT  PART  —  0  |  PLOT  PART  ==  2, 

%  Plot  of  Planned  Track,  Track  after  Mission  Change,  Rendezvous 
%  Point  and  Initial  Track 

figure(lO);  clf 
orient  tall 

plot(Y,X,'b-');grid; 

title('ARIES  Track  -  Actual  and  Planned'); 
xlabel('Y  (meters)');ylabel('X  (meters)'); 
hold; 

plot(New_Y,  New_X,'g-.'); 
p  lot(N  e w_ Y_ W ay_c ,  N  e w_X_ W ay_c ,  'gd') ; 
plot(Y_REMUS,  X  REMUS,  'm'); 

plot([Y_Way_c(  1 )  PrevY_Way_c(  1  )],[X_Way_c(  1 )  PrevX_Way_c(  1  )],'r'); 
for  ik=2:No_tracks, 

p  lot(  [  Y_W ay_c(ik)  Y_  W ay  e  (ik- 1 )] ,  [X_  W ay_c(ik) . . . 
X_Way_c(ik-l)],'r'); 

end; 

legend('Initial  Track  -  ARIES',... 

'Track  After  Modem  Command  -  ARIES', 'Rendezvous  Point',... 

'REMUS  Track','Planned  Track  -  ARIES',4); 
axis([-100  220  -120  60]); 

%  AXIS([-100  250  -80  60]) 

print  -tiff  -depsc  figure  10_wp5 
hold;  zoom  on 
end 
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